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PREFACE 


机 器 人 技术 是 衡量 一 个 国家 科技 创新 和 高 端 制造 业 水 平 的 重要 标志 。 随 着 机 器 人 技术 
的 快速 发 展 , 它 的 应 用 领域 涉及 工业 、 服 务 、 航 空 航天 、 军 事 等 方面 ,因此 需要 一 批 熟练 掌握 
机 器 人 技术 的 创新 型 人 才 。 

传统 的 机 器 人 教材 大 多 侧重 于 对 机 器 人 学 理论 知识 的 探讨 ,往往 涉及 比较 多 的 矩阵 理 
论 , 控 制 理论 的 知识 ,对 于 读者 来 说 比较 抽象 。 另 一 方面 ,这 些 知 识 面 难以 培养 读者 的 动手 
能 力 , 难 以 取得 良好 的 实践 效果 。 因 此 需要 将 理论 与 实践 相 结合 。 本 书 将 机 器 人 学 的 理论 
和 应 用 相 结合 ,一 方面 概要 地 介绍 了 机 器 人 学 的 理论 , 另 一 方面 着 力 于 介绍 机 器 人 的 仿真 和 
编程 技术 。 

在 机 器 人 的 科研 与 工业 应 用 中 ,机 器 人 仿真 与 编程 技术 发 挥 着 无 可 替代 的 作用 ,这 是 因 
为 它 一 方面 能 够 对 机 器 人 控制 算法 进行 检验 测试 , 另 一 方面 给 机 器 人 的 研发 与 测试 提供 一 
个 无 风险 且 稳 定 的 平台 。 本 书 所 使 用 的 工具 包括 MATLAB/Simulink,3 款 常 用 的 机 器 人 
仿真 软件 和 机 器 人 操作 系统 (Robot Operating System,ROS) 。 这 些 工 具 一 方面 可 以 用 于 将 
机 器 人 学 的 理论 知识 进行 实际 验证 与 研究 , 另 一 方面 用 于 对 机 器 人 进行 设计 、 仿 真 与 测试 。 
同时 ,这 些 工 具 大 多 具有 开源 的 特点 ,而 且 它们 相互 之 间 有 方便 快捷 的 接口 ,能 够 发 挥 各 自 
的 优势 ,实现 更 强大 的 仿真 .编程 功能 。 如 今 , 这 些 仿 真 工具 都 已 广泛 应 用 于 机 器 人 设计 、 研 
发 和 科学 研究 等 方面 ,尤其 是 机 器 人 操作 系统 ,成 为 机 器 人 领域 越 来 越 重要 的 应 用 系统 。 

本 书 的 内 容 主 要 分 为 三 篇 ,第 一 篇 介绍 了 基于 MATLAB 机 器 人 工具 箱 的 机 器 人 仿真 ， 
第 二 篇 介绍 了 3 款 常 用 的 机 器 人 仿真 软件 ,第 三 篇 介绍 了 机 器 人 操作 系统 的 基础 和 应 用 。 

在 第 一 篇 中 ,从 机 器 人 学 的 理论 入 手 . 讲 述 了 机 器 人 学 中 的 数学 基础 .机 器 人 运动 学 、 机 
器 人 动力 学 、 机 器 人 控制 和 轨迹 。 然 后 针对 每 一 部 分 理论 内 容 , 介 绍 了 如 何 使 用 MATLAB 
机 器 人 工具 箱 去 解决 相关 的 问题 。 最 后 . 以 作者 的 一 些 科 研 成 果 作 为 实例 ,介绍 了 
MATLAB 机 器 人 工具 箱 在 科研 中 的 应 用 。 

在 第 二 篇 中 ,介绍 了 3 款 机 器 人 仿真 软件 : V-REP Gazebo 和 OpenRAVE。 它 们 作为 
机 器 人 的 仿真 工具 ,能 够 对 机 器 人 及 其 工作 平台 进行 3D 建 模 和 3D 泻 染 ,搭建 与 现实 类 似 
的 机 器 人 模型 ,并 具备 丰富 的 物理 引擎 ,能 够 对 机 器 人 在 虚拟 的 物理 条 件 下 的 运动 进行 
仿真 。 

在 第 三 篇 中 ,主要 介绍 了 机 器 人 操作 系统 。ROS 是 一 个 适用 于 机 器 人 的 开源 的 操作 系 
统 。 它 提供 了 操作 系统 应 有 的 服务 ,包括 硬件 抽象 .底层 设备 控制 .常用 函数 的 实现 、 进 程 间 
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消息 传递 ,以 及 包 管 理 。 它 也 提供 用 于 获取 、 编 译 、 编 写 和 跨 计 算 机 运行 代码 所 需 的 工具 和 
库 函 数 。 这 部 分 主要 讲述 了 ROS 的 概念 ,应 用 和 相关 的 基础 ,然后 以 Baxter 机 器 人 为 应 用 
对 象 ,介绍 了 ROS 在 机 器 人 中 的 相关 编程 技术 。 


本 书 适 用 于 高 校 的 教师 作为 教材 或 教 辅 ,同时 适合 各 层次 的 机 器 人 开发 人 员 和 机 器 人 


爱好 者 阅读 。 教 师 在 以 本 书 作 为 教材 时 ,可 以 利用 本 书 提供 的 工具 ,布置 一 些 能 够 锻炼 学 生 


zlj4 


FREJ ,激励 学 生 创 新 思维 的 课程 作业 。 初 学 机 器 人 学 的 学 生 在 阅读 本 书 时 ,可 以 参考 一 


些 对 于 机 器 人 学 理论 介绍 更 全 面 的 书籍 ,同时 要 利用 好 本 书 介绍 的 工具 ,去 动手 搭建 机 器 人 
模型 或 编写 相关 代码 。 
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由 于 本 书 所 涉及 的 工具 较 多 ,而 且 它 们 之 间 独 立 性 强 , 在 学 习 上 的 关联 性 不 大 。 在 这 
里 ,我 们 分 别 对 每 一 篇 的 内 容 进行 梳理 .并 给 出 一 些 学 习 建议 。 

第 一 篇 ”介绍 了 机 器 人 学 中 的 数学 基础 、 机 器 人 运动 学 、 机 器 人 动力 学 、 机 器 人 控制 和 
轨迹 ,并 介绍 了 如 何 用 MATLAB 机 器 人 工具 箱 进行 相关 问题 的 仿真 。 建 议 读者 在 学 习 该 
部 分 时 ,应 具备 一 定 的 机 器 人 学 基础 ,对 本 章 出 现 的 例子 要 亲自 动手 实践 ,并 尝试 使 用 机 器 
人 工具 箱 去 解决 实际 科研 或 工作 项 目 中 的 问题 。 

第 二 篇 ”介绍 了 三 种 机 器 人 仿真 软件 。 因 为 三 种 软件 都 是 国外 开源 的 机 器 人 仿真 软 
件 , 所 涉及 的 界面 都 为 英文 。 读 者 在 阅读 本 篇 内 容 中 ,应 根据 书 中 介绍 的 方法 ,动手 去 搭建 
一 些 机 器 人 模型 ,并 进行 编程 仿真 。 此 外 ,本 书 的 内 容 参 考 了 软件 用 户 手 册 的 部 分 内 容 , 读 
者 也 可 参考 文中 提供 的 参考 文献 书目 ,进行 更 深入 的 学 习 与 研究 。 在 这 里 ,因为 V-REP 相 
对 简单 友好 ,建议 初学 者 使 用 这 款 开 源 .免费 的 机 器 人 仿真 软件 练习 编程 。 

第 三 篇 ”介绍 了 机 器 人 操作 系统 的 基础 与 应 用 。 读 者 在 进行 本 部 分 内 容 的 学 习 时 ,应 
亲手 去 安装 虚拟 机 、Linux 系统 和 机 器 人 操作 系统 。 此 外 ,可 以 结合 ROS 的 网 站 ( 详 见 
http://wiki. ros. org/cn/ ROS/Tutorials) ,对 ROS 的 基础 进行 全 面 的 学 习 。 此 外 ,这 一 部 
分 介绍 了 ROS 在 Baxter 机 器 人 的 3 个 实例 ,这 一 部 分 适合 研究 生 进行 学 习 探 究 。 有 条 件 
的 读者 可 以 根据 本 书 提供 的 程序 代码 ,对 这 本 部 分 例子 进行 实现 ,并 进行 深入 的 研究 。 
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第 一 篇 ”基于 MATLAB 工 具 箱 
的 机 器 人 仿真 


MATLAB 机 器 人 工具 箱 的 简介 


MATLAB( 和 矩阵 实验 室 ) 是 美国 MathWorks 公司 开发 的 一 种 商业 数 
学 软件 。 它 可 用 于 线性 代数 计算 、 图 形 和 动态 仿真 的 高 级 技术 计算 语言 和 
交互 式 环境 。 如 今 MATLAB 已 经 广泛 应 用 于 大 学 教学 和 科学 研究 。 
MATLAB 的 核心 功能 可 在 各 种 商业 或 开源 的 许可 之 下 通过 应 用 程序 的 特 
定 工具 箱 去 进行 扩展 。 基 量 与 矩阵 是 MATLAB 的 基本 数据 类 型 ,它们 非 
常 适用 于 解决 机 器 人 学 的 相关 问题 。 

本 书 主 要 基于 澳洲 学 者 Peter Corke 开发 的 MATLAB 机 器 人 工具 
箱 , 使 用 MATLAB 对 机 器 人 进行 仿真 。 来 自 昆 士 兰 科技 大 学 的 教授 
Peter Corke 开发 出 了 MATLAB 机 器 人 工具 箱 和 MATLAB 机 器 视觉 工 
具 箱 ,后 者 可 以 用 于 机 器 视觉 相关 颜色 演 染 、 相机 模型 建立 \ 三 维 视 党 和 控 
制 等 方面 的 研究 。 本 书 对 后 者 不 进行 详细 的 令 迷 ,只 涉及 使 用 机 器 人 工具 
箱 进 行 机 器 人 仿真 。 

机 器 人 工具 箱 主 要 用 于 对 传统 的 关节 式 机 器 人 与 移动 机 器 人 的 研究 
和 仿真 ,提供 了 支持 机 器 人 相关 基本 算法 的 功能 集合 ,例如 三 维 坐 标 中 的 
方向 表示 ,运动 学 动力 学 模型 和 轨迹 生成 。 大 多 数 机 器 人 教科 书 中 提出 
的 例子 都 基于 二 连 杆 机 器 人 ,因为 对 二 连 杆 机 器 人 的 分 析 易 于 处 理 。 但 对 
于 实际 中 应 用 最 广泛 的 六 自由 度 机 器 人 ,其 运动 学 和 动力 学 计算 是 复杂 
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的 ,并 且 可 能 难以 求解 。 机 器 人 工具 箱 所 包含 的 功能 使 其 在 二 连 杆 机 器 人 上 的 应 用 十 
分 方便 , 除 此 之 外 , 它 还 包含 着 能 用 于 六 自由 度 ( 或 更 多 ) 机 器 人 的 功能 。 例 如 ,可 以 非 
常 容易 地 研究 有 效 载荷 质量 对 惯性 窍 阵 的 影响 或 者 由 电动 机 看 到 的 关节 惯性 的 变化 。 

机 器 人 工具 箱 使 用 一 种 非常 通用 的 方法 描述 串 行 连接 臂 的 运动 学 和 动力 学 模型 。 
这 些 模型 的 参数 封装 在 MATLAB 的 对 象 中 ,因此 用 户 可 以 使 用 这 些 机 器 人 对 象 去 创建 
各 种 串联 机 械 臂 ,而 其 中 的 可 视 化 仿真 使 抽象 的 机 器 人 学 习 变 得 更 加 直观 。 

在 使 用 机 器 人 工具 箱 时 ,每 个 连 杆 由 连 杆 对 象 (Link Object) 表 示 。 每 个 连 杆 对 象 
的 属性 包括 : 标准 型 (Standard) 或 改进 型 (Modified)Denavit-Hartenberg 参数 ,关节 
和 电机 惯性 值 ,摩擦 和 齿轮 比 等 。 多 个 连 杆 对 象 组 成 机 器 人 对 象 ,在 机 器 人 对 象 上 可 计 
算 诸 如 正 向 和 逆向 运动 学 以 及 前 向 和 逆向 动力 学 等 相关 问题 。 工 具 箱 中 的 示例 对 象 包 
念经 典 的 puma560 机 器 人 、KUKA 机 器 人 和 Baxter 机 器 人 。 本 书 也 将 对 这 些 机 器 人 
进行 简要 介绍 。 

Simulink 是 MATLAB 的 配套 产品 , 它 提供 了 基于 框图 建 模 语言 的 动态 系统 仿真 。 
在 机 器 人 工具 箱 中 ,用 于 工具 箱 函 数 的 封装 模块 能 够 以 框图 形式 描述 非 线性 机 器 人 系 
统 , 让 使 用 者 能 够 研究 自己 设计 的 机 器 人 控制 系统 的 闭环 性 能 。 

除 此 之 外 ,机 器 人 工具 箱 还 提供 了 用 于 处 理 不 同 数据 类 型 之 间 的 转换 工具 ,如 四 元 
数 ( 表 示 三 维 位 置 和 方向 ) 可 通过 转换 工具 运用 齐 次 变换 ,方便 快捷 地 完成 相应 变换 。 

机 器 人 工具 箱 的 优点 包括 : 

(1) 机 器 人 工具 箱 使 用 相对 完善 和 成 熟 的 代码 。 自 1996 年 发 布 的 第 一 个 版 本 到 
2015 年 发 布 的 最 新 版 本 ,工具 箱 已 经 发 布 了 多 个 版 本 ,因此 它 本 身 使 用 的 代码 都 经 过 了 
不 断 的 改进 。 

(2) 机 器 人 工具 箱 中 的 示例 程序 十 分 直观 ,使 用 户 易 于 理解 ; 对 同一 个 算法 提供 了 
不 同 的 实现 方法 ,有 助 于 针对 不 同 的 情况 进行 比较 分 析 。 

(3) 机 器 人 工具 箱 提 供 了 源 人 代码。 用户 在 使 用 时 ,可 以 深入 阅读 其 中 的 源 文件 ,从 
而 对 工具 箱 的 使 用 和 开发 有 更 好 的 理解 。 
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1.1 MATLAB 机 器 人 工具 箱 的 下 载 与 安装 


1. 下 载 

MATLAB 工具 箱 可 从 Peter Corke 提供 的 网 站 上 免费 下 载 ,网 址 为 : http://www. 
petercorke. com/Robotics Toolbox. html, ft Downloading the Toolbox 栏目 中 单 击 here 
按钮 进入 下 载 页 面 ,然后 在 该 页 面 中 填写 国家 、 组 织 和 身份 等 信息 ,进入 机 器 人 工具 箱 的 下 
载 页 面 。 如 图 1-1 所 示 ,在 下 载 页 面 中 , 它 提供 了 机 器 人 工具 箱 的 多 个 历史 版 本 ,本 书 选择 
和 使 用 了 版 本 名 为 robot-9. 10 的 MATLAB 机 器 人 工具 箱 。 


Robot Toolbox download 


robot-9.]0.zip 24 February 2015 20.0 Mbyte 
robot-9, 3, zip 12 February 2012 12.6 Mbyte 
robot-9.4.zip 23 February 2012 12.6 Mbyte 
robot-9.6.zip 22 July 2012 12.9 Mbyte 
robot-9.T.zip 25 September 2012 13.0 Mbyte 
robot-9.8.zip 12 February 2013 13.4 Mbyte 
robot-9.9.zip 28 April 2014 17. 5 Mbyte 


图 1-1 网 站 所 提供 的 机 器 人 工具 箱 版 本 


2. 安装 

将 下 载 后 的 zip 压缩 包 进 行 解压 ,然后 将 名 字 为 “rvctools” 的 文件 夹 存放 在 MATLAB 
的 安装 路 径 下 的 toolbox 文件 夹 里 面 。 

3. 路 径 配 置 

MATLAB 调用 的 函数 必须 在 它 的 搜索 路 径 中 ,因此 需要 将 机 器 人 工具 箱 的 文件 夹 路 
径 添加 到 MATLAB 的 搜索 路 径 。 具 体 的 做 法 是 : 启动 MATLAB. E Set Path 的 界面 中 添 
加 相关 的 机 器 人 工具 箱 路 径 。 如 图 1-2 所 示 , 利 用 MATLAB 工具 栏 里 面 的 Set Path 将 文 
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件 夹 rvctools 里 面 的 文件 夹 设置 为 搜索 目录 .包括 了 ...\rvetools,...\rvectools\common,...\ 


rvctoolsVcontrib ....VrvctoolsVrobot....NVrvctoolsVsimulink 这 5 个 文件 夹 。 


All changes take effect immediately. 


Add Folder... 


Add with Subfolders... 


MATLAB search path: 


NMATLABWATLAB Production Server R2015aMoolboxVwriting 

MMATLABWATLAB Production Server\R201Sa\toolbox\rvctools\simulink 
NMATLABWATLAB Production Server\R2015a\toolbox\rvetools\robot 

MMATLABWATLAB Production Server\R2015a\toolbos\rvctools\contrib 

NMATLAB\MATLAB Production Server\R2015a\toolbox\rvetools\common 
MMATLABWATLAB Production Serve R2015aMoolboxVvctools 

AWATLABWATLAB Production Server\R201Sa\toolbox\hdicoder\matiabhdicoder\matiabhe 
MMATLABWATLAB Production Serve R2015aMoolboxVhdicoderimatlabhdicoder 
NMATLAB\MATLAB Production Server\R2015a\toolbox\matlabximatlabxi 
AWATLABWATLAB Production Server\R201Sa\toolbox\matiabximatiabxidemos 
NMATLABWMATLAB Production Server\R201Sa\toolbox\matlab\addons 

NMATLABWATLAB Production Server\R2015a\toolbox\matlab\addons\cef 
MMATLABWATLAB Production Serve R2015aMoolboxymatlabladdonsValibackmanager 
NMATLABVMATLAB Production Server\R2015a\toolbox\matlab\demos 

MMATLABWATLAB Production Serve R2015aMoolboxynatlablgraph2d 
AWATLABWATLAB Production Server\R201Sa\toolbox\matiab\graph3d 

le 


Save Close. Revert | Default 


图 1-2 MATLAB 工 具 栏 中 对 机 器 人 工具 箱 的 路 径 配置 
4. 机 器 人 工具 箱 的 启动 


在 MATLAB 中 的 command window 输入 rtbdemo, 完 成 机 器 人 工具 箱 的 启动 。 如 
图 1-3 所 示 ,可 以 查看 机 器 人 工具 箱 的 所 有 功能 ,主要 包含 了 关节 机 器 人 和 移动 机 器 人 的 各 


种 功能 。 本 书 将 侧重 于 介绍 机 器 人 工具 箱 在 关节 机 器 人 中 的 应 用 。 


Robotics Toolbox for 
Robot Mobile 


图 1-3 机 器 人 工具 箱 的 启动 界面 
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1.2 机 器 人 学 的 数学 基础 


1.2.1 三 维 空间 中 的 位 置 与 姿态 


通常 来 说 ,机 器 人 指 的 是 至 少 包含 有 一 个 固定 刚体 和 一 个 活动 刚体 的 机 器 装置 。 其 中 ， 
固定 的 刚体 称 为 基 座 , 而 活动 的 刚体 称 为 末端 执行 器 。 在 两 个 部 件 之 间 会 有 若干 连 杆 和 关 
节 来 支撑 末端 执行 器 ,并 使 其 移动 到 一 定 的 位 置 。 

控制 一 个 机 器 人 的 运动 ,可 以 通过 控制 机 器 人 (机 械 臂 ) 上 各 关节 的 位 置 , 设 定 关节 运动 
的 轨迹 。 而 首先 需要 做 的 就 是 获取 机 器 人 本 身 的 位 姿 。 所 谓 位 姿 ,就 是 指 机 器 人 上 每 个 关 
节 在 每 一 时 刻 的 位 置 和 姿态 。 这 就 需要 确定 描述 空间 物体 位 姿 的 方法 ,本 书 中 使 用 空间 坐 
标 系 来 描述 相关 位 姿 。 当 得 到 位 姿 的 描述 以 后 ,就 可 以 利用 各 关节 位 姿 之 间 的 关系 来 描述 
机 器 人 的 整个 运动 链 , 进 而 得 到 机 器 人 的 基 座 坐标 系 和 末端 执行 器 坐标 系 之 间 的 关系 。 

机 器 人 的 运动 学 模型 包括 机 器 人 各 连 杆 , 关 节 的 位 置 姿 态 以 及 在 各 关节 上 的 坐标 系 , 其 
任务 之 一 就 是 确立 机 器 人 未 端 执行 器 的 位 姿 。 机 器 人 的 机 械 臂 通常 是 由 一 组 关节 连接 的 连 
杆 结合 体 : 第 一 个 连 杆 固定 ,连接 该 机 械 臂 的 基 座 ,而 最 后 一 个 连 杆 连接 的 是 它 的 末端 执行 
器 。 操 作 机 器 人 是 为 了 控制 与 机 器 人 相关 的 零件 .工具 在 三 维 空间 中 运动 ,因此 需要 描述 相 
应 的 位 置 和 姿态 。 


1. 位 置 描述 
如 图 1-4 所 示 ,在 三 维 空间 中 建立 某 一 坐标 系 , 于 是 空间 中 的 任何 一 个 点 就 可 以 通过 一 
个 3X1 的 位 置 撩 量 来 确定 。 建 立 一 个 直角 坐标 系 {A), 空 间 中 的 4p, 
任 一 点 ^P 可 以 表示 为 : 
^ i 
ap = |p, G4 
Py 
b. p 
HEP p. bs PIE P 在 坐标 系 {A} 中 的 三 个 坐标 分 量 ,*P 称 
为 位 置 矢量 。 图 1-4 空间 中 点 4P 
的 位 置 描述 


在 MATLAB 中 ,可 以 利用 它 本 身 自 带 的 函数 plot3() 画 出 三 
维 空间 中 的 一 个 点 。 例 如 点 p 的 坐标 为 (1,1,3), 输 入 代码 : 
plot3(1,1,3,'0') 就 用 圆圈 画 出 这 个 点 。 

2. 姿态 描述 

空间 中 的 物体 还 需要 描述 它 的 姿态 (也 称 为 方位 ), 这 用 固定 在 物体 上 的 坐标 系 {B} 来 
描述 。 如 图 1-5 所 示 ,为 了 规定 空间 某 刚 体 B 的 方位 , 设 一 坐标 系 {B) 与 此 刚体 固 连 ; 用 三 
个 单位 矢量 xn: yn ,zs 来 表示 坐标 系 {B} 的 主轴 方向 ,因此 物体 相对 于 参考 坐标 系 {A} 的 姿 
态 可 以 用 矢量 xo ,ys ,zs 相对 于 参考 坐标 系 {A} 的 方向 余弦 组 成 的 3X3 和 矩阵 来 表示 ,这 个 矩 
VESR 称 为 旋转 矩阵 。 
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ru nz rys 
R= [xs ^ys ^zs]— |ra rm rz (1.2) 


31 Ts Ts 


用 矢量 两 两 之 间 的 余弦 则 表示 为 : 


COs(rA.rp) cos(ra.yp) cosCra.zp) 
ÎR = eoo cos( ya. yn) een (1,3) 
cos(zA.rnB)  cos(za.ym)  cos(za.zp) 
zh 
{4} 
Ya 
Xi 
图 1-5 空间 中 某 刚体 B 的 姿态 描述 
对 应 于 轴 X,Y 或 Z 作 转 角 为 0 的 旋转 变换 ,其 旋转 矩阵 分 别 为 ; 
n 0 0 ] 
R.(0)— |0 cosü 一 sin0 (1. 4) 
LO sing cosó! 
f cos 0 sin0] 
R,CO) — 0 1 0 (1.5) 
L— sin 0 cos0J 
[cos — sinü 0] 
R.(0) — | sin0 cosü 0 2.6) 
L 0 0 1J 
旋转 矩阵 8R 具有 这 样 的 特点 : 
(D 3 个 主 矢量 两 两 垂直 ; 
(2) 9 个 元 素 中 ,只 有 3 个 是 独立 的 ; 
G) 3 个 单位 主 矢量 满足 6 个 约束 条 件 : 
Ayg *^xg —^Ayg «yg = zp zp = 1 (5,7) 
^x p ^yn —^ yn zn —^xp «^c — 0 (1.8) 


(4) 旋转 矩阵 为 正 交 矩阵 ,并 且 满足 条 件 : 
ig^ - je. |iRi- 1 
在 机 器 人 工具 箱 中 ,可 分 别 用 函数 rotx(0) ,roty(0) ,rotz(0) 计 算 旋转 0 的 旋转 矩阵 ,其 中 ,在 
默认 情况 下 ,0 用 弧度 表示 。 
例如 围绕 X 轴 做 转角 为 180" 的 旋转 变换 时 ,输入 MATLAB 命令 : 
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>> R= rotx(pi) 


1. 0000 0 0 
0 -1.0000 -0.0000 
0 0.0000 一 1.0000 


如 果 直 接 用 角度 0 表示 ,可 以 分 别 用 rotx(0,'deg') ,roty(0,'deg') ,rotz(0,'deg') 计 算 旋 
转 矩 阵 。 上 面 例 子 的 MATLAB 命令 : 


>> R= rotx(180, 'deg') 


1.0000 0 0 
0 -1.0000 -—0.0000 
0 0.0000 一 1.0000 


以 上 两 种 方式 ,都 可 以 得 到 相对 应 的 旋转 矩阵 。 

在 机 器 人 工具 箱 中 ,可 以 使 用 两 种 函数 实现 坐标 的 旋 
转 可 视 化 。 函 数 trplot() 可 以 用 图 形 表示 相应 的 体 坐 标 
Z PŽ tranimate() 用 动画 展示 世界 坐标 系 旋转 为 体 坐 
标 系 的 过 程 。 

输入 trplot R) ,运行 结果 如 图 1-6 所 示 ; 输入 tranimate(R). 
运行 结果 如 图 1-7 所 示 。 

3. 本 节 函 数 解析 

根据 Peter Corke 所 编著 的 MATLAB 机 器 人 工具 箱 的 
函数 说 明文 档 ( 即 Robotics Toolbox for MATLAB ) ,以 下 对 
本 节 所 出 现 过 的 函数 进行 进一步 的 解析 。 图 1-6 使 用 trplot() 生 成 的 坐标 图 

1) 获取 旋转 矩阵 

A) rotx(): R=rotx(0) 是 表示 围绕 X 轴 旋 转 弧度 为 0 得 到 的 旋转 矩阵 ,返回 一 个 3X3 

(2) roty(): R—roty CO ERR [80 £& Y 轴 旋 转 弧度 为 得 到 的 旋转 矩阵 ,返回 一 个 3X3 
的 矩阵 。 

(3) rotz(): R=rotz(0) 是 表示 围绕 Z 轴 旋 转 弧 度 为 9 得 到 的 旋转 矩阵 ,返回 一 个 3X3 
的 矩阵 。 
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图 1-7 使 用 tranimate() 生 成 的 坐标 图 


以 上 三 个 函数 中 ,可 选 参数 为 "deg”, 表 示 角 度 值 单位 为 度 (degree) 。 

2) 绘制 坐标 系 

函数 为 : trplot() 。 

对 于 三 维 坐 标 系 的 绘制 ,机 器 人 工具 箱 提供 了 强大 的 可 视 化 函数 trplot() 。 

(1) trplotCRO : 绘制 由 旋转 矩阵 得 到 的 坐标 系 , 其 中 坐标 系 根据 正 交 旋转 矩阵 围绕 原 
点 旋转 得 到 ,R 为 3X3 的 矩阵 。 

(2) trplot(T) : 绘制 由 齐 次 变换 矩阵 表示 的 三 维 坐 标 系 ,其 中 T 了 为 4X4 的 矩阵 。 

此 外 , 它 包 含 了 许多 可 选 参数 ,这 里 列举 主要 的 几 种 : 


参数 E x 2 — "* E x 
i ces 在 绘图 上 不 显示 坐标 轴 i 将 图 形 显示 的 轴 尺 寸 设置 为 A, 其 中 
A= [xmin xmax ymin ymax zmin zmax] 
x 设置 轴 的 颜色 ,C 代表 将 绘制 出 来 的 坐标 系 命名 为 F, 并 且 
'color', C 'frame', F » 
MATLAB 图 形 内 置 的 颜色 类 型 X.Y.Z 轴 的 下 标 含有 下 
调整 显示 文本 的 字体 大 小 等 属 


设置 绘图 视图 参数 V — [az el] 角 度 ， 


"text opts'. opt | 性 ; fj 如 { ' FontSize ' , 10, | 'view', V WEAR MURIS acto! 


'FontWeight'. 'bold') 


"ewgth's s | 坐标 轴 的 长 度 (默认 值 1) varow“ 设置 坐标 轴 的 未 端 为 箭头 ,而 不 是 线段 
‘width', w 箭头 宽度 (默认 为 1) "thick', t 线条 粗细 (默认 0. 5) 
MUT ; 3r "sd" WE REDIG HEU JO CT 
ad a EERE "anaglyph', A | 符 (默认 颜色 为 “re”): 选 自 红 , 绿 , 蓝 ， 
T 青 , 品 红 
"dispar'，D 3d 显示 差异 (默认 0.1) "text" 启用 在 框架 上 显示 X,Y,Z 标签 
| 以 红色 ,绿色 , 蓝 色 分 别 显示 X.Y,Z 轴 


个 ,第 3 个 字符 标记 X,Y,Z 轴 
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3) 动画 展示 

PARC: tranimate()。 

(1) tranimate(xl,x2,options) 展 示 3D 坐标 系 从 姿态 xl 变换 到 姿态 x2 的 动画 效果 。 
其 中 ,姿态 xl 和 x2 有 三 种 表示 方法 : 一 个 4X4 的 齐 次 矩阵 ,或 一 个 3X3 的 旋转 矩阵 ,或 
一 个 四 元 数 。 

(2) tranimate(x，options) 展 示 了 坐标 系 由 上 一 个 姿态 变换 到 姿态 x 的 动画 效果 。 同 
样 地 ,姿势 x 也 有 三 种 表示 方法 : 一 个 4X4 的 齐 次 矩阵 ,或 一 个 3X3 的 旋转 矩阵 ,或 一 个 
四 元 数 。 

(3) tranimate( xseq，options) 展 示 了 移动 一 段 轨迹 的 动画 效果 。xseq 可 以 是 一 组 4X 
AXN 的 齐 次 矩阵 ,或 一 组 3X3XNN 的 旋转 矩阵 ,或 是 一 组 四 元 数 向 量 (NX1) 。 

它 包 含 的 可 选 参数 如 下 : 


$ —* 意 x 
'nsteps's n | 沿路 径 的 步 数 (默认 500 
将 帧 保存 为 文件 夹 M 中 的 文件 ,M 为 
文件 的 路 径 


参 数 意 义 
"fps… fps | 每 秒 显 示 的 帧 数 (默认 为 10) 
设置 三 个 轴 的 边界 , A = [ xmin. 


xmax, ymin, ymax, zmin, zmax] 


'movie', M 


1.2.2 坐标 变换 


在 三 维 空间 中 , 任 一 坐标 系 均 可 作为 描述 一 个 物体 位 姿 的 参考 坐标 系 。 在 机 器 人 学 的 
许多 问题 中 ,常常 需要 在 不 同 的 参考 坐标 系 中 表示 同一 个 物体 的 位 姿 。 例 如 ,图 1-8 的 应 用 
例子 中 ,需要 在 相机 的 指导 下 ,使 用 机 器 人 的 手臂 去 夹 取 一 个 目标 物体 。 因 此 ,需要 获取 世 
界 坐 标 系 、 固 定 相 机 的 坐标 系 、 机 器 人 的 坐标 系 、 机 器 人 的 相机 坐标 系 \ 目 标 物体 的 坐标 系 的 
位 置 与 姿态 ,使 得 机 器 人 的 末端 执行 器 能 够 达到 夹 取 目标 物体 所 需要 的 位 姿 , 执 行 夹 取 
任务 。 


{固定 的 相机 


{0} 世 界 坐 标 系 
图 1-8 机 器 人 夹 取 物 体 任务 中 的 坐标 系 


1. 平移 坐标 变换 
如 图 1-9 所 示 , 坐 标 系 {A} 没 有 经 过 旋转 ,只 经 过 平移 得 到 坐标 系 {B}。P 是 B 坐标 系 
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中 的 一 点 ,用 矢量 ?5P 表示 它 在 坐标 系 {B} 中 的 位 置 ; “Psorc 表 示 A 坐标 系 平移 到 B 坐标 系 
的 距离 。 当 P 用 坐标 系 {A} 表 示 时 ,用 矢量 ^P 表示 。 因 为 矢量 *P AP 具有 相同 的 姿态 ， 
所 以 可 以 使 用 矢量 相 加 的 方法 表示 和 : 

^P —"P +P sorc (1.9) 


图 1-9 坐标 系 {O4) 经 过 平移 变换 到 坐标 系 {Os) 
用 4X4 的 齐 次 矩阵 表示 平移 变换 矩阵 工 , 则 


l 0 0 p 
01i € 
f= i (0.10 
0 0 1 p: 
0 0 0 1 


其 中 ,p: ,py 和 p: 是 平移 向 量 P 相对 于 参考 坐标 系 X 轴 、Y 轴 和 2 轴 的 3 个 分 量 。 与 矢量 和 
旋转 矩阵 一 样 ,坐标 系 还 可 以 用 上 述 变换 算 子 表示 。 
对 于 平移 坐标 变换 ,机 器 人 工具 箱 提供 了 函数 transl() 计 算 一 段 平移 相对 应 的 平移 变 


换 矩 阵 。 
例如 ,空间 中 的 一 个 坐标 系 {A), 它 可 以 表示 为 
0.527 一 0.574 0.628 5 
| 0.369 0.819 0.439 3 
| [—0.766 0 0.643 8 
0 0 0 1 


如 果 将 这 个 坐标 系 沿 着 参考 坐标 系 的 Y 轴 移 动 10 个 单位 ,然后 再 沿 着 Z 轴 移 动 5 个 
单位 得 到 坐标 系 {B) , 求 坐标 系 {B} 的 表示 。 
输入 命令 : 


>> A=[0.527, —0.574,.628,5; 
0.369,0.819,0.439,3; 
— 0. 766, 0,0. 643,8; 
0,0,0,1]; 

>> T= trans1(0,10,5) ; 

>> B=T#*A 
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运行 结果 : 


B= 
0.5270 一 0.5740 0.6280 5.0000 
0.3690 0.8190 0.4390 13.0000 

— 0. 7660 0 0.6430 13.0000 
0 0 0 1.0000 


T- 
a d 
o3 or 10 
i o» e c ses 
0. 9 9 m" 

2. 旋转 坐标 变换 


如 图 1-10 所 示 , 当 坐标 系 {A} 没 有 经 过 平移 ,只 经 过 旋 
转 时 (旋转 矩阵 为 8R) ,得 到 坐标 系 {B)。 同 一 个 点 了 在 坐标 
系 {A} 和 {1B} 中 的 表示 分 别 为 *P 和 P, 这 两 个 撩 量 有 这 样 的 
变换 关系 : 

^p —$R?P 

机 器 人 工具 箱 分 别提 供 了 trotxO ,troty() «trotzO — 
函数 计算 旋转 变换 矩阵 ,分 别 对 应 着 X 轴 ,Y 轴 和 2Z 轴 旋 转 
一 定 的 角度 ,得 到 的 是 一 个 4X4 的 矩阵 。 

例如 上 面 的 坐标 系 {A} ,然后 绕 着 X 轴 旋 转 30^ , 求 旋 转 


" : : 图 1-10 坐标 系 {OA} 经 过 旋转 
后 得 到 的 些 标 系 人 L 变换 到 坐标 系 1Os) 
运行 指令 : 


>> T= trotx(pi/6) 


运行 结果 
T= 
1.0000 0 0 0 
0 0.8660  —0.5000 0 
0 0.5000 0.8660 0 
0 0 0 1.0000 


得 到 旋转 变换 矩阵 后 ,可 运行 下 面 指令 得 到 坐标 系 {C} 的 表示 : 


2» C-T*A 
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0.5270 一 0.5740 0.6280 5.0000 
0.7026 0.7093 0.0587 -1.4019 
— 0.4789 0.4095 0.7764 8.4282 
0 0 0 1.0000 


3. 齐 次 坐标 变换 
如 图 1-11 所 示 , 当 坐标 系 {A} 经 过 平移 (距离 为 *Paonc ) 与 旋转 (旋转 矩阵 为 银 ) 得 到 坐 
标 系 { 了 B}。 同 一 个 点 P ERRA AMB) PRRI P 和 5, 这 两 个 矢量 有 如 下 的 
变换 关系 : 
^P —$R?P +P sorc Q.1D 


Xa 
图 1-11 包括 了 平移 变换 和 旋转 变换 的 一 般 坐 标 变换 


从 上 式 可 以 得 出 在 不 同 坐 标 系 中 的 位 姿 *P 和 ?P 之 间 的 关系 ,但 在 上 式 中 ^P MP 并 不 
是 齐 次 变换 的 关系 ,结果 只 能 简易 计算 相 邻 坐标 系 中 位 姿 之 间 的 关系 , 当 不 是 相 邻 甚至 需要 
得 到 末端 执行 器 和 基 座 坐标 系 上 位 姿 的 关系 时 ,计算 量 太 大 、 太 复杂 ,所 以 应 该 将 这 个 等 式 
写成 齐 次 变换 的 形式 。 因 为 在 三 维和 矩阵 计算 不 能 充分 描述 齐 次 变换 ,所 以 增加 了 和 矩阵 的 维 
数 , 将 等 式 改写 为 四 维 矩 阵 的 形式 , 即 : 


ap SR ^P pore | |"P. 
(1)= M IG) .12) 
将 转换 因子 用 齐 次 变换 矩阵 氏 表示 , 即 : 
ÎR AP Borc 
ir- |。 : | (1.13) 
1x3 
因此 上 式 可 以 表示 为 
^p —jT^P a. 10 
对 于 旋转 和 矩阵 外 ,根据 正 交 矩阵 的 性 质 可 以 得 到 : 
AR 4R — je" a.15) 


而 对 于 转换 矩阵 ,其 逆 和 矩阵 不 等 于 原 矩 阵 . 经 过 推导 ,可 得 到 : 
ADT — ApTA 
$r3 —T = E 2d cem (1.16) 


第 1 章 ”机 器 人 学 与 MATLAB 机 器 人 工具 箱 |» 13 


因此 ,得 到 了 相 邻 坐标 系 间 矢量 (旋转 矢量 和 移动 矢量 ) 的 齐 次 变换 。 

前 面 提 到 ,对 于 平移 坐标 变换 ,机 器 人 工具 箱 提供 了 函数 transl(); 对 于 旋转 坐标 变换 ， 
机 器 人 工具 箱 分 别提 供 了 函数 trotx() \troty() ,trotz()。 下 面 用 例子 说 明 如 何 用 机 器 人 工 
具 箱 解决 齐 次 变换 的 问题 。 

[B 1-1] 已 知 坐标 系 {A} 与 坐标 系 {B} 初 始 位 姿 重 合 ,首先 {B} 相 对 于 {A} 的 ya 旋转 
60^ ,再 沿 着 {A} 的 za 轴 移 动 4 个 单位 ,最 后 沿 着 {A} 的 =a 轴 移动 3 个 单位 。 

COD 点 pi 在 坐标 系 {B} 中 的 描述 为 *p1 二 [2 4 3]J", 求 它 在 坐标 系 {A} 中 的 描述 *p1。 

(2) 另 有 一 点 ps 在 坐标 系 {A} 中 的 描述 为 4ps 二 [2 4 3】, 求 它 在 坐标 系 {B} 中 的 描 


述 3p,。 
解析 : 
OD 旋转 矩阵 争 =RCy,60") ,平移 矢量 为 *ps 二 [4 0 3] 
F ÈR ia 
BT = 
Oixs 1 
输入 命令 : 
>> T= trans1(4,0,3) * troty(pi/3) 
运行 结果 为 : 
T= 
0.5000 0 0.8660 4.0000 
0 — 1.0000 0 0 
一 0.8660 0 0.5000 3.0000 
0 0 0 — 1.0000 


点 pi 在 坐标 系 {A} 中 的 描述 为 : ^P —OT"P. 
输入 命令 : 


> p=T x+ [2;4;3;1] 


运行 结果 为 : 
p= 
7.5981 
4. 0000 
2.7679 
1. 0000 


(2) 由 P== 包 3P 可 得 到 到 一 条 -4P。 
输入 命令 : 
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运行 结果 为 : 


关于 三 维 空间 中 的 坐标 系 ,MATLAB 机 器 人 工具 箱 提供 了 函数 trplot0 〇 ) 可 以 画 出 该 坐 
标 系 。 对 于 上 面 的 例子 ,可 以 通过 trplotO) 画 出 坐标 系 {A} 和 坐标 系 {B)。 
输入 命令 : 


运行 结果 如 图 1-12 所 示 。 


图 1-12 通过 函数 trplot() 生 成 的 坐标 系 


此 外 ,关于 齐 次 变换 矩阵 负 ,机 器 人 工具 箱 提供 了 函数 t2r() 可 以 提取 旋转 矩阵 分 量 。 
输入 命令 : 


第 1 章 ”机 器 人 学 与 MATLAB 机 器 人 工具 箱 |» 15 


运行 结果 为 
RS 
0.5000 0 — 0.8660 
0 — 1.0000 0 
— 0. 8660 0 — 0.5000 


而 函数 rz2t() 则 可 以 将 旋转 矩阵 转换 成 对 应 的 齐 次 变换 矩阵 。 如 通过 运行 指令 r2t(R) 
可 以 得 到 上 面 的 矩阵 T. 

通过 以 下 的 命令 可 以 提取 平移 变换 分 量 。 

输入 命令 : 


>> p= tranl(T) 
运行 结果 : 


p= 
4 
0 
3 


4. 本 节 函 数 解析 

根据 Robotics Toolbox for MATLAB ,以 下 对 本 节 所 出 现 过 的 函数 进行 进一步 的 解析 。 

1) 平移 变换 transl() 

(1) 使 用 transl() 创 建 平 移 变 换 矩 阵 。 

T = transl(x. y, 2» : 表示 能 够 获取 一 个 分 别 沿 着 ny ez 轴 平 移 一 段 距离 得 到 的 4X4 
齐 次 变换 矩阵 。 

T = transl(p) : 表示 由 经 过 矩阵 (或 向 量 )p = [z,y'z] 的 平移 得 到 的 齐 次 变换 矩阵 。 
WR p 为 (MX3) 的 矩阵 , 则 T 了 为 一 组 齐 次 变换 和 矩阵 (4X4X MD E TG: s: DIEF p 
的 第 i 行 。 

(2) 使 用 transl() 提 取 一 个 矩阵 中 的 平移 变换 分 量 。 

[x,y'z] = transl T): x. yz 是 齐 次 变换 矩阵 中 的 三 个 分 量 ,是 一 个 1XM 的 向 量 。 

p = transl( T); p 是 齐 次 变换 矩阵 中 T 的 平移 部 分 ,是 一 个 3XM 的 矩阵 。 

2) 旋转 坐标 变换 

(D T=trotx(0): 表示 围绕 X 轴 旋 转 0( 弧 度 ) 得 到 的 齐 次 变换 矩阵 (4X4) 。 

(2) T=troty(0): 表示 围绕 Y 轴 旋转 0( 弧 度 ) 得 到 的 齐 次 变换 矩阵 (4X4) 。 

(3) T=trotz(0): 表示 围绕 Z 轴 旋 转 0( 弧 度 ) 得 到 的 齐 次 变换 矩阵 (4X4) 。 

以 上 3 个 函数 中 ,可 选 参数 为 deg: 表 示 角 度 值 单位 为 度 (degree) 。 

3) t2r() 

R = t2r(T) 获 取 齐 次 变换 矩阵 中 正 交 旋 转 和 矩阵 分 量 。 如 果 T 是 一 个 4X4 的 矩阵 ， 
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W R 是 一 个 3X3 的 矩阵 ; WR T J&—- 3: 3 的 矩阵 , 则 R 是 一 个 2X2 的 矩阵 。 

4) r2t() 

函数 r2t() 可 将 旋转 矩阵 转换 为 齐 次 变换 矩阵 。 

T 二 r2t(R) 获 取 一 个 正 交 旋转 矩阵 R 等 价 的 具有 零 平 移 分 量 的 齐 次 变换 矩阵 。 如 果 R 
是 一 个 3X3 的 矩阵 , 则 了 是 一 个 4X4 的 矩阵 ; 如 果 R 是 一 个 2X2 的 矩阵 , 则 了 是 一 个 3X3 
的 矩阵 。 


1.2.3 姿态 的 其 他 表示 方法 


1. 角 坐 标 表示 法 
角 坐 标 表示 法 的 原理 是 : 首先 考虑 坐标 系 {B) 和 一 个 固定 的 参考 坐标 系 {A}) 重 合 , 然 后 
每 次 将 {B} 绕 着 {A} 或 {1B) 的 其 中 一 条 轴 旋 转 一 定 的 角度 ,需要 连续 转 三 次 。 角 坐标 表示 法 
有 24 种 典型 的 表示 方法 : 每 次 旋转 都 只 围绕 着 {A} 的 某 条 轴 , 共 有 12 种 , 称 为 固定 角 坐 标 
系 法 ;而 每 次 旋转 都 是 绕 着 {B) 的 某 条 轴 , 共 有 12 种 , 称 为 欧 拉 角 坐 标 系 法 。 下 面 介绍 几 种 
常用 的 方法 。 
D X-Y-Z B fü A bi 
X-Y-Z 固定 角 坐 标 系 的 方法 : 首先 将 坐标 系 {B} 和 固定 的 参考 坐标 系 {A} 重 合 , 然 后 
(1B}) 绕 着 Xs 旋转 7 , 青 绕 着 Ys 旋转 8B, 最 后 绕 Z4 旋 转 a。 相 对 于 义 .Y 和 2 轴 旋 转 的 角 分 别称 
为 回转 角 、 俯 仰角 和 偏转 角 。 这 里 可 以 通过 三 个 旋转 矩阵 的 相 乘 推导 出 经 过 一 系列 旋转 后 
的 等 价 旋转 矩阵 : 
ÎR xyz (7.B,C)= Rz(a) Ry (QD Rx OP (1.12 
从 而 可 以 得 出 : 
ER xyz CY Bia) 
cosa。cosB cosa * sinf* siny — sina + cosy cosae。sin8。cosy 十 sinae。siny 
= | sinae。cosB sinae。sin8。siny 十 cosae。cosy sina * sing + cosy — cosa * siny 
— sing cos * siny cosf * cosy 
(1.18) 
可 以 看 出 ,上 式 的 结果 比较 复杂 ,而 机 器 人 工具 箱 提供 了 简单 的 函数 去 计算 等 价 的 旋转 矩 
阵 ,其 中 一 种 是 使 用 之 前 提 过 的 函数 rotx() roty ORI rotz() 按 顺序 相 乘 ; 另外 一 种 是 使 用 
函数 rpy2r()。 
例如 ,在 XYZ 固定 角 坐 标 系 中 , 当 回 转角 y 为 30 ,俯仰 角 8 为 45°, 偏 转角 a 为 607 
时 , 求 出 经 过 这 一 系列 旋转 变换 得 到 的 旋转 矩阵 : 
输入 命令 


>> R1 = rotz(pi/3) x roty(pi/4) x rotx(pi/6) 
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或 者 
>> R1 = rpy2r(pi/3, pi/4, pi/6, 'zyx') 


该 函数 默认 的 是 X YZ ,应 在 后 面 的 选项 添加 zyx 指定 为 ZYX 顺序 。 
运行 结果 : 
Be 

0.3536 -0.5732 0.7392 


0.6124 0.7392 0.2803 
—0.7071 0.3536 0.6124 


如 果 已 知 一 个 旋转 矩阵 ,要 求 出 对 应 的 偏转 角 、 俯 仰角 和 回转 角 , 根 据 理论 计算 很 难得 
到 所 要 求 的 值 。 在 机 器 人 工具 箱 中 ,可 以 根据 函数 tr2rpy() 计 算出 一 个 给 定 旋转 矩阵 的 偏 
转角 、 俯 仰角 和 回转 角 。 

例如 ,在 上 面 中 的 例子 求 出 的 旋转 矩阵 R1, 可 以 求 出 对 应 的 三 个 角 、 输 入 命令 : 


>> TB= tr2rpy(R1, 'zyx') 
运行 结果 : 


TB = 
1.0472 0.7854 0.5236 


因此 ,可 以 得 出 ,回转 角 y 为 0. 5236 ,俯仰 角 8 为 0.7854, 偏 转角 a 为 1.0472( 注 意 顺 
序 )。 

2) ZY-Z 欧 拉 角 坐 标 系 

Z-Y-Z 欧 拉 角 坐标 系 的 方法 : 首先 将 坐标 系 {B} 和 固定 的 参考 坐标 系 {A} 重 合 ,然后 
{B} 绕 着 Zs 旋转 a fü ,再 绕 着 Ya 旋转 B, 最 后 绕 Za 旋 转 y。 这 里 可 以 通过 三 个 旋转 矩阵 的 相 
乘 推导 出 经 过 一 系列 旋转 后 的 等 价 旋 转 矩 阵 : 


BRzvy Or-[C) = Rz GORy GQDRz Cy) Q. 19) 
从 而 可 以 得 出 : 
ÈR zyx Cr 4C) 
cosa * sinf * cosy — sina * siny — cosa * cos * siny — sina * cosy cosa » sinf 


= |sina * cos * cosy + cosa * siny sina * cos * siny + cosa * cosy sina * sing 
— sinf* cosy sing * siny cos 
(1. 20) 
同样 地 ,机 器 人 工具 箱 提供 了 函数 eul2r() 去 计算 对 应 的 欧 拉 角 。 
例如 ,在 Z-Y-Z 欧 拉 角 中 ,坐标 系 {B) 绕 Zs 旋转 30 ,再 绕 Ys 旋转 45 ,最 后 绕 Za 旋 转 
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60 ,要求 经 过 这 一 系列 旋转 变换 得 到 的 旋转 矩阵 。 
输入 命令 ; 


>> R2 = rotz(pi/6) * roty(pi/4) * rotz(pi/3) 


或 者 


>> R2 = eul2r(pi/6, pi/4, pi/3) 


—0.1268  —0.7803 0.6124 
0.9268 0.1268 0.3536 
— 0.3536 0.6124 0.7071 


此 外 ,如 果 已 知 一 个 旋转 矩阵 ,要求 出 对 应 的 三 个 欧 拉 角 ,在 机 器 人 工具 箱 中 ,可 以 根据 
函数 tr2eul() 计 算出 一 个 给 定 旋转 矩阵 的 欧 拉 角 。 
输入 命令 : 


>> EA = tr2eul (R2) 
运行 结果 : 


EA= 
0.5236 0.7854 1.0472 


因此 可 以 得 出 ,三 个 七 YZ 欧 拉 角 分 别 是 0.5236、0.7854 和 1.0472。 

2. 向 量 表示 法 

在 机 器 人 学 中 ,用 向 量 去 表示 一 个 坐标 系 的 方法 比较 少 用 ,但 机 器 人 工具 箱 提供 了 相关 
的 函数 进行 计算 。 

1) 双向 量 表示 法 

双向 量 表示 法 是 假设 一 个 旋转 矩阵 为 


Ws Os d. 
R= |n, o, a, 
2. 0. d. 


已 知 两 个 在 同一 个 平面 但 不 平行 的 向 量 a 二 [a a, az] 和 向 量 o 一 [or o, o.]. 
因为 两 个 向 量 确定 了 一 个 平面 ,即使 它们 之 间 不 存在 正 交 的 关系 ,但 可 以 通过 又 乘 来 确定 另 
一 个 新 的 向 量 n, 而 且 向 量 n 垂直 于 它们 所 确定 的 平面 。 

在 机 器 人 工具 箱 中 ,提供 了 函数 oa2r() 去 计算 两 个 非 平行 的 向 量 所 定义 的 一 个 坐标 


(1. 21) 
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系 。 例 如 ,两 个 向 量 分 别 是 a— [2.1.0 ffl o— [1.2.0 ] ,计算 对 应 的 旋转 矩阵 。 
输入 指令 : 


2»a7[2,1,0]; 
»»0-[1,2,0]; 
>> R= oa2r(o,a) 


运行 结果 : 


R = 
0 一 0.4472 0.8944 

0 0.8944 0.4472 

—1.0000 0 0 


2) 单 向 量 旋转 法 

对 于 空间 中 任意 两 个 姿态 的 坐标 系 , 可 以 在 空间 中 找到 某 个 轴 ,使 得 其 中 一 个 坐标 系 绕 
着 这 个 轴 旋 转 一 定 的 角度 ,与 另外 一 个 坐标 系 姿态 重合 。 其 中 这 个 向 量 称 为 旋转 向 量 ,与 前 
面 提 到 的 旋转 矩阵 ,二 者 可 以 通过 罗 德 里 格 斯 变换 进行 转换 : 


0 =r Ks 
R = cosól 十 ames cin re 0 —r. (1. 22) 
—r, f 


其 中 ,r 为 旋转 向 量 ,0 为 对 应 的 角度 。 

机 器 人 工具 箱 中 ,提供 了 函数 angvec2r() 将 向 量 与 它 所 选择 的 角度 转换 到 等 价 的 旋转 
和 矩阵。 例如 ,坐标 系 A 绕 着 X 轴 旋 转 90`, 求 它 等 价 的 旋转 矩阵 。 

输入 指令 : 


>> R= angvec2r(pi/2,[1,0,0]) 


1. 0000 0 0 
0 0.0000 -— 1.0000 
0 1.0000 0.0000 


提供 了 函数 angvec2tr() 将 向 量 与 它 所 选择 的 角度 转换 到 等 价 的 齐 次 变换 矩阵 。 以 上 
面 的 例子 为 例 , 输 入 指令 : 


>> T= angvec2tr(pi/2,[1,0,0]) 
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运行 结果 
T = 
1.0000 0 0 0 
0 0.0000 -— 1.0000 0 
0 1.0000 0.0000 0 
0 0 0 1.0000 


如 果 进 行 逆 运 算 , 求 出 旋转 矩阵 所 对 应 的 旋转 向 量 及 角度 ,可 以 使 用 函数 trZangvecO 。 
例如 上 面 例子 的 逆 运 算 ,输入 指令 : 


>> [theta, v] = tr2angvec(R) 


运行 结果 
theta = 

1.5708 

H 0 0 
3. 本 节 函 数 解 析 


根据 Robotics Toolbox for MATLAB ,下 面 对 本 节 所 出 现 过 的 函数 进行 进一步 的 解析 o 

1) rpy2r() 

R = rpy2r(roll, pitch,，yaw，options) 能 够 根据 一 组 回转 角 、 俯 仰角 和 偏转 角 (1X3) 求 
出 对 应 齐 次 变换 矩阵 中 的 旋转 矩阵 R(3X3) ,其 中 3 个 角度 rpy 二 [R,P,Y] 分 别 对 应 于 关于 
XY 和 Z 轴 的 顺序 旋转 。 

R = rpy2r(rpy， options) 与 前 者 相同 ,但 是 输入 的 是 一 个 向 量 (1X3)。 如 果 rpy 是 
NX3, 那 么 它们 表示 为 一 段 轨迹 ,得 到 的 旋转 矩阵 R 是 三 维 矩 阵 (3X3X NN)。 

函数 rpy2r() 包 含 的 可 选 参数 如 下 : 


‘deg’ 用 角度 去 计算 (默认 为 弧度 ) 
‘zyx’ 返回 关于 Z.Y «X 轴 的 顺序 旋转 的 解 


2) tr2rpyO 

rpy = tr2rpyCT. options) 能 够 将 齐 次 变换 矩阵 转换 为 对 应 的 回转 角 、 俯 仰角 和 偏转 
角 。 其 中 ,T 为 一 个 4X4 的 矩阵 ,rpy 为 齐 次 变换 矩阵 中 的 旋转 部 分 对 应 的 回转 角 、 俯 仰角 
和 偏转 角 。 其 中 ,3 个 角度 roy - LR. P, Yj] 分别 对 应 于 围绕 XY 和 Z 轴 的 顺序 旋转 。 

rpy = tr2rpyCR. options) 与 上 面相 似 .但 输入 的 和 矩阵 R 是 一 个 3X3 的 矩阵 。 

如 果 R(3X3XK) 或 T(4X4XK) 表 示 一 个 序列 , 则 rpy 的 每 一 行 对 应 于 该 序列 的 一 组 
回转 角 、 俯 仰角 和 偏转 角 。 
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函数 tr2rpy() 包 含 的 可 选 参 数 如 下 : 


"deg' 用 角度 去 计算 (默认 为 弧度 ) 
'zyx' 返回 关于 Z,Y,X 轴 的 顺序 旋转 的 解 
3) eul2r() 


函数 eul2r() 的 主要 功能 是 将 一 组 欧 拉 角 转换 为 旋转 矩阵 。 

R = eul2r(phi,theta,psi,options) 是 能 够 获得 一 组 指定 的 欧 拉 角 对 应 的 正 交 旋转 矩阵 
(3X3)。 其 中 输入 的 三 个 欧 拉 角 分 别 对 应 于 围绕 Z.Y.Z 轴 旋 转 的 角度 。 如 果 phi, theta, 
psi 是 列 向 量 (NX1), 则 假设 它们 表示 一 段 轨迹 ,得 到 的 旋转 矩阵 R 是 三 维 矩 阵 (3X3Xx 
N), R = eul2rCeul,options) 与 上 面 的 相似 ,但 是 欧 拉 角 是 从 矩阵 的 列 向 量 中 取得 的 。 如 
果 eul 是 矩阵 (NX3), 则 它们 表示 一 段 轨迹 ,得 到 的 旋转 矩阵 R 是 三 维和 矩阵 (3X3XN)。 

函数 eul2r() 的 可 选 参数 为 'deg', 用 角度 表示 。 

4) tr2eul() 

函数 tr2eul() 的 主要 功能 是 将 齐 次 变换 矩阵 转换 为 欧 拉 角 。 

eul = tr2eul(T,options) 是 对 应 于 齐 次 变换 矩阵 T(4X4) 的 旋转 部 分 XY 和 2Z 轴 欧 
拉 角 (1X3)。 获 得 的 向 量 eul = [PHI, THETA, PSI].3 个 角度 分 别 对 应 于 围绕 XY HZ 
轴 的 顺序 旋转 。 

eul = tr2eul(R,options) 与 上 面相 似 , 但 输入 是 一 个 正 交 旋转 矩阵 R(3X3)。 如 果 R(3X 
3XK) 或 T(4X4XK) 表 示 一 个 序列 ,那么 eul 的 每 一 行 对 应 于 该 序列 的 每 一 步 。 

函数 tr2eul() 包 含 的 可 选 参数 如 下 : 


'deg' 用 角度 去 计算 (默认 为 弧度 ) 
'flip' 选择 第 一 个 欧 拉 角 在 第 2 象限 或 第 3 象限 
5) oa2r() 


函数 oa2r() 能 够 将 方向 和 接近 向 量 转换 为 等 价 的 旋转 矩阵 。 

R = oa2rCo. a) 对 于 由 3 个 向 量 形成 的 指定 取向 和 接近 向 量 (3X1),R==oa2r(o,a) 是 
SO(3) 旋 转 矩 阵 (3-~3) ,使 得 R=[N oa]. 

6) angvec2r() 

函数 angvec2r() 能 够 将 角度 和 向 量 方向 转换 为 等 价 的 旋转 矩阵 。 

R = angvec2r(theta,v) 能 够 得 到 绕 矢量 wv 旋转 theta 角度 而 得 到 对 应 的 正 交 旋 转 矩 阵 
R(GX3)5, 

7) angvec2tr() 

函数 angvec2tr() 能 够 将 角度 和 向 量 方向 转换 为 等 价 的 齐 次 变换 矩阵 。 

T = angvec2tr(theta,v) 是 等 效 于 围绕 向 量 v 旋转 0 角度 的 齐 次 变换 矩阵 (4X4) 。 
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8) tr2angvec() 

函数 tr2angvec() 能 够 将 旋转 矩阵 转换 为 等 价 的 角 和 矢量 形式 。 

[theta.v] = tr2angvec(R,options) 能 够 求 出 正 交 旋转 矩阵 R(3X3) 相 应 等 价 的 向 量 和 向 
量 所 需要 旋转 的 角度 。[Ltheta,v] = tr2angvec(T,options) 与 上 面相 类 似 , 但 用 齐 次 变换 表示 旋 
转 部 分 。 


1.2.4 具体 例子 的 应 用 
【 例 1-2】 如 图 1-13 所 示 ,一 个 单 连 杆 操作 臂 的 手腕 具有 一 个 自由 度 。 


1 0 2 
1 6 

已 知 手 部 起 始 位 次 条 阵 为 G, 一 |” 。 1 ;| AFIA Zo 轴 施 转 十 90, 则 手 部 到 
0. 0 0 1 


达 Gs; 若 手 臂 不 动 , 仅 手 部 绕 手腕 Z 轴 旋 转 十 90 ， 
则 手 部 到 达 Gs; 写 出 手 部 坐标 系 {G:} 及 {Gs ) 的 矩阵 
解析 : 
(1) 手臂 绕 定 轴 转 动 是 相对 固定 坐标 系 作 旋转 
变换 , 故 有 
G: = Rot(Z.90* )G; 
(2) 手 部 绕 手腕 轴 转 动 是 相对 动 坐标 系 作 旋 转 


G; = Gi Rot( Z.90*) 
程序 代码 如 下 : 图 1-13 一 个 单 连 杆 操作 臂 结构 


G1-[0102; 
1006; 
QO= 
0001]; 
和 名 名 问题 一 求解 : 
G2 = trotz(pi/2) * G1; 
% 名 名 问题 二 求解 : 
G3 = G1 x trotz(pi/2); 


可 以 得 到 运行 结果 : 


G2 = 
一 1.0000 0.0000 0 -6.0000 
0.0000 1.0000 0 2.0000 
0 0 -—1.0000 2.0000 
0 0 0 1.0000 
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G3 = 
0.0000 一 1.0000 0 2.0000 
一 1.0000 一 0.0000 0 6.0000 
0 0 一 1.0000 2.0000 
0 0 0 1.0000 


[B 1-3] 如 图 1-14 所 示 ,通过 人 的 手腕 握 住 WII 遥控 器 转动 ,从 而 控制 机 器 人 的 手 
腕 ,让 机 器 人 的 手腕 跟随 还 控 器 进行 转动 。 然 而 , WII 遥控 器 的 取向 是 根据 横 滚 -俯仰 - 偏 航 
(RPY) 角 度 给 出 ,而 机 器 人 的 手 的 取向 是 使 用 ZYZ 欧 拉 角 , 此 时 就 需要 进行 RPY 角 与 
ZYZ 欧 拉 角 进行 转换 。 


HR -Z 


图 1-14 WIGER 2855 BL28 A. FR ERRA T 388 08 00 A I fa fa MEA 
右 图 表示 了 机 器 人 手腕 旋转 ,手腕 弯曲 和 手 部 旋转 ) 

问题 : 编写 一 个 MATLAB 函数 ,使 得 给 出 一 组 RPY 角 能 够 返回 一 组 与 之 对 应 的 欧 拉 
角 。 从 旋转 矩阵 确定 一 组 欧 拉 角 有 两 种 解决 方案 : 第 一 种 方案 即 通过 普通 的 旋转 ,第 二 种 
方案 即将 第 二 次 旋转 约束 在 0 一 180` 之 间 ,然后 进行 旋转 。 

解析 : 

第 一 方案 : 通过 机 器 人 工具 箱 提供 的 rpy2eulO ,可 以 将 PRY 角 直 接 转化 为 ZYZ 欧 拉 
角 , 它 将 返回 一 个 向 量 ,这 个 向 量 对 应 于 分 别 围绕 Z、Y 和 2 轴 旋 转 的 三 个 角 。 

第 二 种 方案 : 

由 一 组 欧 拉 角 (phi,theta,psi) 给 出 的 旋转 矩阵 可 以 表示 为 : 


R = rotz(phi) x roty(theta) * rotz(psi) 
在 这 种 方案 中 : 

roty(theta) = rotz(pi) * roty( - theta) * rotz(pi) 
因此 ,旋转 矩阵 为 : 


R = rotz(phi) * rotz(pi) * roty( 一 theta) * rotz(pi) * rotz(psi) 
= rotz(phi * pi) * roty( ~ theta) x rotz(psi + pi) 
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程序 代码 如 下 : 


多 通过 sigma 的 设置 ,决定 两 种 方案 

functioneuler = rpy2eul(rpy, sigma) 

% 方 案 一 : 直接 通过 函数 计算 欧 拉 角 

euler = tr2eul(rpy2tr(rpy)); 

s HR: 通过 R= rotz(phi * pi) x roty( - theta) * rotz(psi+ pi) 计 算 欧 拉 角 


if sigma == -1 

euler(1) = euler(1) + pi; 
euler(2) = - euler(2); 
euler(3) = euler(3) + pi; 
end 

end 


1.3. 机 器 人 运动 学 


1.3.1 机 械 臂 及 运动 学 


1. 机 械 辟 的 构成 

机 器 人 本 体 , 是 机 器 人 赖 以 完成 作业 任务 的 执行 机 构 ,一 般 是 一 台 机 械 臂 ,也 称 操作 臂 
或 操作 手 , 可 以 在 确定 的 环境 中 执行 控制 系统 指定 的 操作 。 典 型 工业 机 器 人 本 体 一 般 由 手 
部 (末端 执行 器 )、 腕 部 、 辟 部 ,腰部 和 基 座 构成 。 机 械 辟 多 采用 关节 式 机 械 结构 ,一般 具有 6 
个 自由 度 ,其 中 3 个 用 来 确定 末端 执行 器 的 位 置 ,另外 3 个 则 用 来 确定 末端 执行 装置 的 方向 
(姿势 ) 。 机 械 臂 上 的 末端 执行 装置 可 以 根据 操作 需要 换 成 焊 枪 .吸盘 、 扳 手 等 作业 工具 。 

如 图 1-15 所 示 ,一 个 机 械 臂 是 由 一 组 可 做 相对 运动 的 关节 连接 的 连 杆 结合 体 。 第 一 个 
连 杆 固 定 ,连接 该 机 械 臂 的 基 座 ,而 最 后 一 个 连 杆 连接 的 是 它 的 末端 执行 器 。 


图 1-15 机 械 臂 的 构成 


通常 可 将 关节 划分 为 两 种 : 第 一 种 称 为 转动 关节 (或 称 为 旋转 关节 ) ,转动 关节 可 绕 基 
准 轴 转动 ,相应 的 转动 量 称 为 关节 角 ; 第 二 种 称 为 移动 关节 ,移动 关节 是 沿 着 基准 轴 移 动 ， 
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相应 的 位 移 称 为 关节 偏 距 。 还 有 一 种 特殊 的 关节 称 为 球 关节 , 球 关节 拥有 三 个 自由 度 , 可 以 
三 个 转动 关节 和 一 个 零 长 度 的 连 杆 来 描述 一 个 球 关节 。 
位 于 机 械 臂 固定 基 座 的 坐标 系 称 为 基 坐 标 系 ; 位 于 操作 臂 末端 执行 器 的 坐标 系 称 为 工 

具 坐 标 系 ,通常 用 它 来 描述 机 械 臂 的 位 置 。 

2. 机 器 人 运动 学 的 定义 

运动 学 是 研究 物体 的 运动 ,而 不 考虑 物体 的 质量 以 及 引起 这 种 运动 的 力 。 如 图 1-16 所 
示 , 机 器 人 正 运 动 学 是 已 知 或 给 定 一 组 关节 角 , 计 算出 工具 坐标 系 相 对 于 基 坐 标 系 的 位 置 和 
姿态 ,也 就 是 说 ,用 正 运动 学 来 确定 机 器 人 未 端 执行 器 的 位 姿 。 机 器 人 逆 运 动 学 是 给 定 机 械 
臂 末 端 执行 器 的 位 置 和 姿态 ,计算 所 有 可 到 达 给 定位 置 和 姿态 的 关节 角 。 也 就 是 说 ,未 端 执 
行 器 在 特定 的 一 个 点 具有 特定 的 姿态 ,去 计算 出 它 所 对 应 的 每 一 关节 变量 的 值 。 机 器 人 运 
动 学 的 研究 方法 ,首先 利用 位 姿 描 述 .坐标 系 变换 等 数学 方法 确定 物体 位 置 .姿态 和 运动 ; 
然后 确定 不 同 结构 类 型 的 机 器 人 的 正 逆 运 动 学 ,这些 类 型 包括 直角 坐标 型 ,圆柱 坐标 型 和 球 
坐标 型 等 等 ; 最 后 根据 Denavit-Hartenberg 参数 法 去 推导 机 器 人 的 正 逆 运动 学 方程 。 


FA 
工具 坐标 系 (Tool) 


图 1-16 ”机械 臂 的 正 运动 与 道 运动 示意 图 


1.3.2 DH 参数 法 


DH 参数 全 称 为 Denavit-Hartenberg 参数 , 它 使 用 连 杆 参数 来 描述 机 构 运 动 关 系 。 如 
图 1-17 Bros ,在 标准 型 DH 参数 法 中 ,描述 机 械 臂 中 的 每 一 个 连 杆 需要 4 个 运动 学 参数 ,分 
别 是 连 杆 长 度 aia 、 连 杆 转角 aia EFT fi RT Y f 0 它们 的 定义 如 下 : 

。 连 杆 长 度 ai_1: 关节 轴 i 一 1 和 关节 轴 i 之 间 公 垂 线 的 长 度 。 

。 连 杆 转角 aii: 第 i 一 1 个 关节 轴 和 第 i 个 关节 轴 之 间 的 夹 角 。 

。 连 杆 偏 距 d;: 沿 两 个 相 邻 连 杆 公共 轴线 方向 的 距离 。 

。 关节 角 O: 两 相 邻 连 杆 绕 公共 轴 线 旋转 的 夹 角 。 
以 上 4 个 参数 对 应 转动 关节 和 移动 关节 有 两 种 情况 : 转动 关节 中 , 连 杆 长 度 、 连 杆 转 
角 和 连 杆 偏 距 是 固定 不 变 的 ,关节 角 6: 为 变量 ; 移动 关节 中 , 连 杆 长 度 、 连 杆 转 角 和 关节 和 角 
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是 固定 不 变 的 , 连 杆 偏 距 4; 为 变量 。 


图 1-17 DH 参数 法 示意 图 


1. 创建 一 个 连 杆 对 象 
在 MATLAB 机 器 人 工具 箱 中 ,还 用 变量 oi 表示 机 器 人 的 关节 类 型 ,其 中 oi 二 0 表示 转 


动 关节 ,oi 二 1 表示 移动 关节 ( 若 未 指定 该 参数 ,默认 为 转动 关节 )。 在 工具 箱 中 ,用 函数 
Link() 可 以 创建 一 个 机 械 臂 对 象 ,其 中 输入 的 参数 顺序 分 别 是 关节 和 角 0;、 连 杆 偏 距 di;、 连 杆 
长 度 aia 、 连 杆 转角 aia 、 关 节 类 型 。 


例如 ,创建 一 个 关节 角 初 始 为 0 三 0, 连 杆 偏 距 4; 二 2, 连 杆 长 度 aia 三 3, 连 杆 转角 


aii —45* ,关节 类 型 为 转动 关节 的 连 杆 。 


输入 命令 : 


>> L= Link([0,2,3,pi/4,0]) 


L= 
theta=q, d=2, a=3, alpha = 0.7854, offset = 0 (R, stdDH) 


其 中 ,offset 表示 关节 的 偏 移 量 。R 表示 为 旋转 关节 、stdDH 表示 用 标准 型 DH 参数 法 去 描 


述 。 


用 以 下 的 命令 可 以 获取 连 杆 的 各 个 参数 。 

。 获取 连 杆 的 关节 类 型 : L. RP; 

。 获取 连 杆 的 关节 角 : L. theta: 

。 获取 连 杆 的 连 杆 偏 距 : L. d; 

t 获取 连 杆 的 连 杆 长 度 : L. a; 

。 获取 连 杆 的 连 杆 转角 : L. alpha, 

2. 创建 一 个 具有 n 自由 度 的 机 械 臂 

创建 一 个 平面 三 杆 机 械 臂 ,因为 它 的 3 个 关节 均 为 转动 关节 ,所 以 有 时 称 该 机 械 臂 为 


RRR( 或 3R) 机 构 。 
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三 连 杆 平面 机 械 臂 的 DH 参数 如 下 表 所 示 : 


连 杆 0: di(m) aii Gn) aii 
1 br 0 1 0 
2 0; 0 0.8 0 
3 [A 0 0.6 0 
输入 命令 : 


>>L(1) = Link([0,0,1,0]); 
>>L(2) = Link([0,0,0.8,0]); 
> L(3) = Link([0,0,0.6,0]); 
^L 


theta- gl, d- 0, a- 1, alpha- 0, offset- 0 (R,stdDH) 

theta = q2, d- 0, a7 0.8, alpha- O0, offset = 0 (R, stdDH) 

theta = q3, d=0, a7 0.6, alpha- 0, offset - 0 (R,stdDH) 

38 xt F3 PR C SerialLink() 可 以 给 创建 的 机 械 臂 对 象 命名 ,并 显示 出 对 象 的 信息 。 
输入 命令 : 


>> three link = SerialLink(L, 'name', 'threelink') 


jid : 
three link = 
threelink (3 axis, RRR, stdDH, slowRNE) 
gocce dee LARE i a speed re ii 
| jl theta | d| al alpha | offset | 
qee—pee——————- doe esent rm ple E ÀÀ M * 
1| q1| ol 1| ol ol 
| 2| q2| ol 0.8| o| ol 
| 3l a3| ol 0.6| o] ol 
4--—4----------- 4------—---- 4----—------ 4---—------- 4-—--------- * 
grav = 0 base=1 0 0 0 tool= 1 0 0 0 
0 0251220250 | > O 00 
9.81 0 0 1 0 0200193530 
guo ONT y e i01 


这 里 的 std 表示 该 机 械 臂 根据 标准 型 DH 参数 进行 定义 ,重力 加 速度 默认 作用 在 Z 轴 
上 ,为 9.81N/kg, 基 上 坐标 系 和 工具 坐标 系 保存 初始 的 位 姿 。 
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用 以 下 的 命令 可 以 获取 已 创建 机 械 臂 的 各 个 参数 : 


参数 命 令 参 数 命 d 
关节 数目 three link. n xt fü three link. theta 
DH 参数 three link, links 连 杆 偏 距 three_link. d 
关节 类 型 three link. config 连 杆 长 度 three. link. a 
连 杆 转角 three link. alpha 


同时 也 可 以 对 创建 的 机 械 臂 对 象 进行 复制 ,如 复制 一 个 名 称 为 "three_link2” 的 机 械 臂 。 
输入 命令 : 


>> L2 = SerialLink(three link, 'name', 'threelink2') 


4--—4----------- 4----------- 4----------- 4----------- 4----------- * 
Toni theta | d| al alpha | offset | 
4--—4----------- 4----------- 4----------- 4----------- 4----------- * 
Wal all ol i| ol ol 
2| q2| ol 0.8| ol 0| 
| 3l al ol 0.6] ol ol 
4--—4----------- 4----------- 4----------- 4----------- 4----------- * 

grav = 0 base=1 00 0 tool= 100 0 

0 Dos 0 r 00 

9.81 0010 0/20: 71..9 

0/50: 01 0:10:03 


1.3.3 ”机 器 人 正 运动 学 

1.3.3.1 连 杆 坐标 中 的 DH 参数 

首先 应 该 确定 两 个 关节 上 的 坐标 系 ,方法 步骤 如 下 : 

CD 将 两 个 关节 轴 方 向 都 定 为 第 i 个 坐标 系 和 第 i 一 1 个 坐标 系 的 Z 轴 。 

O 将 两 个 关节 轴 之 间 公 垂 线 方向 定 为 第 i 个 坐标 系 的 X 轴 , 而 在 公 垂 线 第 ;个 关节 轴 
上 的 交点 作为 原点 。 同 理 , 第 i 一 1 个 坐标 系 的 X 轴 和 原点 由 之 前 两 个 坐标 系 公 垂 线 共 同 
确定 。 

O 在 已 经 确定 坐标 系 Z 轴 和 XX 轴 的 情况 下 ,坐标 系 的 Y 轴 直 接 通过 右手 定 则 来 确定 。 

对 于 初始 轴 ( 第 0 个 轴 ) ,其 与 基 座 有 关 . 可 以 自由 定义 ,甚至 可 以 与 第 1 个 关节 轴 重 合 ， 
这 样 使 第 一 个 连 杆 长 度 ao 、 第 一 个 转角 w 置 零 , 同 时 会 简化 正 运动 学 的 计算 。 
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确定 坐标 系 之 后 ,可 以 对 DH 参数 做 进一步 描述 : 
。 EKE aim: zi; 轴 方 向 上 x; 轴 和 xzi+1 轴 之 间 的 距离 。 
* 连 杆 转角 aii: zx: 轴 方向 上 x; 轴 和 zi+1 轴 之 间 的 夹 角 。 
* 连 杆 偏 距 d;: x; 轴 方向 上 zi;_1 轴 和 xz; 轴 之 间 的 距离 。 
。 关节 角 O: z; 轴 方向 上 xz;_1 轴 和 zi; 轴 之 间 的 夹 角 。 
1.3.3.2 相 邻 连 杆 之 间 的 变换 
1. 标准 型 DH 参数 描述 法 
在 机 器 人 的 每 个 关节 上 DH 参数 中 的 四 个 参数 分 别 代 表 关 节 连 杆 不 同 的 特征 或 在 进行 
不 同 的 变换 。 如 图 1-18 所 示 ,在 某 一 瞬间 ,从 第 i 一 1 个 关节 到 第 i 个 关节 ,经 历 的 变换 有 : 
Z 轴 旋 转 (0:) Z 轴 平 移 (di) «X IDEE Cau RI X ET Cai. 
kti- 关节 i Xl 
NO HH i ` 


图 1-18 标准 的 DH 参数 的 示意 图 
将 之 前 的 齐 次 变换 矩阵 工 和 DH 参数 联系 起 来 , 即 可 得 到 以 下 公式 : 


HiT = Rz(0:) Dz(d:) Dx(ai1) R, Gia? (1. 23) 
cosü; 一 Sinbicosai sinĝ;sina;ı aiicosO: 
sin; cosbicosarl  — cosĝ;sinaiı arsin; 
HT = . (1. 24) 
0 sinas COsai 1 di 
0 0 0 1 


MATLAB 机 器 人 工具 箱 用 矩阵 A 表示 工 , 用 函数 L. A() 求 出 上 一 节 创建 的 连 杆 对 象 


dí A 
>> L= Link([0,2,3,pi/4,0]); 
>>L.A(0) 
运行 结果 
ans = 
1.0000 0 0 3.0000 


0 0.7071 -0.7071 0 
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0 0.7071 0.7071 2.0000 
0 0 0 1.0000 


所 以 这 个 变换 矩阵 为 ; 
1 0 0 3 
sí 10 0.7071 —0.7071 0 
AT- 5 0.7071 0.7071 2 
0 o 0 1 
对 于 给 定 机 器 人 的 连 杆 ,坐标 系 (站 相对 应 坐标 系 {i 一 1) 的 变换 是 只 有 一 个 变量 的 函数 
( 即 旋转 关节 的 关节 角 0: 或 移动 关节 的 连 杆 偏 距 di) 。 
当 上 面 创建 的 连 杆 对 象 关节 角 & 为 30 "时 ,可 用 函数 L.A(C) 求 出 相应 的 变换 矩阵 。 
输入 命令 : 


>L.A(pi/6) 


运行 结果 为 : 


ans = 
0.8660 一 0.3536 0.3536 2.5981 
0.5000 0.6124 -0.6124 1.5000 
0 0.7071 0.7071 2.0000 
0 0 0 1.0000 


求 出 相应 的 变换 矩阵 为 : 
0.866 一 0.3536 0.3536 2.5981 
0.5 0.6124 — 0.6124 1.5 
0 0.7071 — 0.7071 2 
0 0 0 1 


FT 一 


2. 改进 型 DH 参数 描述 法 
改进 型 DH 参数 描述 法 对 坐标 系 变换 如 图 1-19 所 示 ,在 某 一 瞬间 ,从 第 i 一 1 个 关节 到 
第 i 个 关节 ,经 历 的 变换 有 : X ETE Ca), X 轴 平 移 (a;_1)、Z 轴 旋 转 (0;) 和 Z 轴 平 移 
(di)。 所 以 ,对 应 的 变换 矩阵 为 : 
AT = Rzları) Dy (a1) Rz Dz(d) 


cos; — sinĝ; 0 aia 
sinf;cosa;;, cosbicosarl 一 Sinarl  — sinas d; 
aT -— B à . 
sinÜ;sina;;  cosÜ;sina;4 COsai 1 cosa; d; 


0 0 0 1 
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A x, QE 


图 1-19 ”改进 的 DH 参数 的 示意 图 


在 创建 连 杆 对 象 时 ,使 用 改进 型 DH 描述 法 需要 指定 参数 为 modified. 
输入 命令 : 


>> L= Link([0, 2, 3, pi/4, 0], modified') 
运行 结果 : 


L= 
theta=q, d=2, a=3, alpha- 0.7854, offset = 0 (R,modDH) 


modDH 表示 使 用 了 改进 后 的 DH 参数 描述 法 ,输入 命令 : 


»»L.A(0) 


1.0000 0 0 3.0000 
0 0.7071 -0.7071 -1.4142 
0 0.7071 0.7071 1.4142 


0 0 0 1.0000 
所 以 这 个 变换 矩阵 为 
1 0 0 3 
: 0 0.70721 —0.7071 — 1.4142 
AT- |o 0.7071 0.7071 — 1.4142 
0 o 0 1 


1.3.3.3 连续 的 连 杆 变换 
通过 将 每 一 个 连 杆 的 变换 矩阵 连 乘 能 够 得 到 坐标 {N} 相 对 于 坐标 10} 的 变换 矩阵 ， 
ST SiTi TNT 
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这 个 变换 矩阵 是 N 个 关节 变量 的 函数 。 

回顾 正 运动 学 的 概念 : 给 定 一 组 关节 角 , 计 算出 工具 坐标 系 相 对 于 基 坐 标 系 的 位 置 和 
姿态 。 在 这 里 ,可 以 通过 各 个 关节 位 置 传感器 得 到 所 需要 的 值 , 然 后 求 出 每 个 连 杆 的 变换 氟 
阵 , 通 过 上 式 就 可 求 出 机 器 人 未 端的 工具 坐标 系 相 对 于 基 坐 标 系 的 位 姿 , 可 表示 为 : 


ru ne rs Pa 


0 0.0 1 

上 面 的 等 式 中 的 ~, 即 三 行 三 列 的 子 矩阵 代表 从 基 座 到 末端 执行 器 的 旋转 矩阵 ,其 中 的 
每 列 从 左 到 右 分 别 代表 末端 执行 器 描述 基 座 中 X 轴 、Y 轴 和 2 轴 方 向 上 的 单位 矢量 , 即 可 
表示 末端 执行 器 基于 基 座 坐标 系 的 方向 姿态 。 而 p 三 行 一 列 从 上 往 下 分 别 代 表 末 端 执行 
器 相对 于 基 座 坐标 系 的 位 置 。 

MATLAB 机 器 人 工具 箱 中 用 了 函数 fkine() 计 算 正 运 动 学 的 问题 。 以 上 面 的 三 连 杆 
平面 机 械 臂 为 例 , 用 标准 型 DH 参数 描述 法 计算 。 

输入 命令 : 


2» L(1) = Link([0,0,1,0]); 

2» L(2) = Link([0,0,0.8,0]); 

»»L(3) = Link([0,0,0.6,0]); 

>> three link = SerialLink(L, 'name', 'threelink'); 
>> T-three link.fkine([0 0 0]) 


运行 结果 
T= 
1.0000 0 0 2.4000 
0 1.0000 0 0 
0 0 1.0000 0 
0 0 0 1.0000 
所 以 初始 状态 时 : 
100 2.4 
à 010 0 
T= 
00 1 0 
000 1 


通过 下 面 的 语句 可 以 将 创建 的 机 械 臂 用 图 像 化 显示 出 来 。 输 入 命令 : 


three link.plot([000]) 


运行 结果 如 图 1-20 所 示 。 
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当 第 二 个 关节 旋转 30 ,第 三 个 关节 旋转 45 时 ,输入 命令 : 


>> T= three link. fkine([0 pi/6 pi/4]) 


运行 结果 
T = 
0.2588 -0.9659 0 — 1.8401 
0.9659 0.2588 0 0.9796 
0 0 1.0000 0 
0 0 0 1.0000 
所 以 : 
0.2588 一 0.9659 0 1.8481 
" 0. 9659 0.2588 0 0.9796 
3T 一 
0 0 1 0 
0 0 0 
输入 命令 : 


>> three link.plot([0 pi/6 pi/4]) 


运行 结果 如 图 1-21 所 示 。 


图 1-20 使 用 DH 参数 法 创建 的 机 械 臂 图 1-21 经 过 旋转 变换 的 机 械 臂 


1.3.4 机 器 人 道 运动 学 
1.3.4.1 逆 运 动 学 的 解 


机 器 人 道 运动 学 的 问题 即 已 知 机 械 辟 末端 的 工具 坐标 系 相对 于 基 坐 标 系 的 位 置 和 姿 
态 , 计 算 所 有 能 够 到 达 给 定位 置 和 姿态 的 关节 角 , 即 已 知 变换 矩阵 MT, 计算 出 能 够 得 到 AT 
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的 一 系列 关节 角 01 ,0，… ,0,。 

对 于 以 上 的 问题 ,有 以 下 几 种 情况 : 

CD 不 存在 相应 的 解 。 当 所 期 望 的 位 姿 离 基 坐 标 系 太 远 , 而 机 械 辟 不 够 长 时 ,末端 执行 
器 无 法 达到 该 位 姿 ; 当 机 械 臂 的 自由 度 少 于 6 个 自由 度 时 , 它 将 不 能 达到 三 维 空 间 的 所 有 
位 姿 ; 此 外 ,对 于 实际 中 的 机 械 臂 ,关节 角 不 一 定 能 到 达 360", 使 得 它 不 能 达到 某 些 范围 内 
的 位 姿 。 在 以 上 的 情况 中 ,机 械 臂 都 不 能 达到 某 些 给 定 的 位 姿 , 因 此 不 存在 解 。 

(2) 存在 唯一 的 解 。 当 机 械 臂 只 能 从 一 个 方向 达到 期 望 的 位 次 时 ,只 存在 一 组 关节 和 角 
使 得 它 能 到 达 这 个 位 姿 , 即 存在 唯一 的 解 。 

CD 存在 多 个 解 。 当 机 械 臂 能 从 多 个 方向 达到 期 望 的 位 姿 时 ,存在 着 多 组 关节 角 能 使 
得 它 到 达 这 个 位 姿 , 即 存在 多 个 解 。 此 时 ,需要 选择 一 组 最 适合 的 解 : 一 是 要 考虑 机 械 臂 从 
初始 位 姿 移 动 到 期 望 位 姿 的 “最短 路程 ”, 得 到 相应 的 解 ; 二 是 要 考虑 在 机 械 臂 移动 的 过 程 
中 是 否 会 遇 到 障碍 ,应 选择 无 障碍 的 一 组 解 。 

1.3.4.2. 逆 运 动 学 的 解法 

对 机 器 人 的 运动 学 方程 进行 求解 ,是 一 个 非 线性 问题 。 目 前 对 这 个 问题 的 求解 方法 分 
为 两 种 : 封闭 解法 和 数值 解法 。 封 闭 解法 不 需要 进行 迭代 ,就 可 以 对 不 高 于 四 次 项 的 多 项 
式 进行 求解 ,存在 代数 法 和 几何 法 这 两 种 方法 ; 数值 解法 的 求解 过 程 需 要 迭代 ,因此 求解 的 
速度 较 慢 。 

因 以 上 的 方法 无 通用 的 公式 解 ,本 书 对 于 以 上 解法 的 数学 方法 不 做 具体 的 详 述 ,下 面 使 
H MATLAB 机 器 人 工具 箱 对 封闭 解 和 数值 解 进行 介绍 。 

工具 箱 中 用 M 文件 的 形式 存储 了 许多 种 类 型 机 器 人 的 DH 参数 等 信息 ,如 KUKA 
KR5,puma560,FanuclOL, Fifi] KUKA KR5 和 puma640 为 例 , 对 机 器 人 的 道 运动 学 进 
行 解析 。 

1. 封闭 解 的 方法 

在 封闭 解法 中 ,使 用 ikine6s() 求 解 逆 运动 学 的 问题 , 它 只 适用 于 关节 数 为 6, 且 腕 部 三 
个 旋转 关节 的 轴 相 交 于 一 个 点 的 情况 。 这 种 解法 使 用 显 式 控 制 的 方法 对 机 械 臂 运动 学 进行 
配置 ,使 得 在 存在 多 个 解 的 情况 下 ,能 够 指定 一 定 的 配置 ,得 到 一 个 唯一 的 解 。 函 数 ikine6s() 


使 用 以 下 标志 符 进行 相应 的 配置 : 
左 K a ns k iyi 
肘 部 向 上 "ut 肘 部 向 下 wd 
腕 部 翻转 Lu LIZS Ll "nt 


以 KUKA KR5 BLAR A 28 [5] ub BL di A D 3t iz 2 ^7 [o S HE CIRCE TAS AIR f o 
加 载 机 器 人 KR5 模型 ,输入 命令 : 


>> ndl KR5 
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运行 结果 ,MATLAB 的 工作 空间 加 载 了 机 器 人 的 参数 ,如 图 1-22 所 示 。 


图 1-22 MATLAB 工 作 空间 中 显示 的 机 器 人 参数 
为 显示 机 器 人 的 具体 参数 ,输入 命令 : 


运行 结果 为 : 


首先 ,使 用 正 运动 学 的 方法 ,让 机 器 人 按 下 列 的 关节 角 旋 转 , 达 到 一 定 的 位 姿 了 ,输入 
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以 工 为 已 知 条 件 , 用 封闭 解 的 方法 求 出 相应 的 旋转 关节 角 , 输 入 命令 : 


>> ql = KR5. ikine6s(T) 


3.1775 3.1416 1.5259 1.0472 


3.1416 -3.3226 
可 以 看 到 ,得 到 的 一 组 关节 角 与 之 前 的 关节 角 不 一 样 ,但 这 两 组 关节 角 显 然 能 够 达到 相 


同 的 末端 执行 器 位 姿 。 此 时 可 以 指定 配置 ,得 到 一 个 目标 解 。 
输入 命令 : 
>> q2 = KR5. ikine6s(T, 'run') 

0.5236 1.0472 


q2 = 
0 0.7854 -0.0000 


分 别 用 KR5. plot(q2) fll KR5. plot(ql) 可 以 生成 以 下 图 形 。 如 图 1-23 所 示 ,可 以 看 出 ， 
虽然 关节 角 不 同 , 但 末端 执行 器 的 位 姿 相 同 。 


图 1-23 ”封闭 解 -两 组 不 同 的 关节 角 使 得 末端 执行 器 达到 同样 的 位 姿 


2. 数值 解 的 方法 
数值 解 的 方法 使 用 ikine() 求 解 逆 运动 学 的 问题 , 它 可 适用 于 各 种 关节 数目 的 机 械 辟 ， 


通过 设 定 初始 的 关节 角 坐标 对 机 械 臂 运动 学 配置 进行 隐 式 控制 。 
以 puma560 机 器 人 为 例 , 对 机 器 人 的 道 运动 学 问题 进行 数值 解法 的 求解 。 


加 载 机 器 人 模型 ,并 输入 一 组 关节 角 ,输入 命令 : 


2» mdl puma560; 
>>qn= [0, pi/4, pi, 0, pi/4,0]; 
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>> T= p560. fkine(qn) 
T= 
— 0. 0000 0.0000 1.0000 0.5963 
— 0. 0000 1.0000 -0.0000 -0.1501 
—1.0000 -0.0000 -0.0000 -0.0144 
0 0 0 1.0000 


未 设 定 初始 关节 角 坐 标 , 使 用 ikine ) 进 行 求解 ,输入 命令 : 
>> qi = p560. ikine(T) 
运行 结果 为 : 


ai= 
-0.0000 -0.8335 0.0940 0.0000 -0.8312 -0.0000 


设 定 初始 关节 角 坐 标 , 使 用 ikine() 进 行 求解 ,输入 命令 : 
>> q2 = p560. ikine(T,[0 0 30 0 0]) 
运行 结果 为 : 


a2 = 
一 0.0000 0.7854 3.1416 -0.0000 0.7854 0. 0000 


同样 地 ,分 别 用 p560. plot(q2) ffl p560. plot(q1) 可 以 生成 以 下 的 图 形 。 如 图 1-24 所 
示 , 可 以 看 出 ,虽然 关节 角 不 同 ,但 末端 执行 器 的 位 姿 相 同 。 


图 1-24 数值 解 -两 组 不 同 的 关节 角 使 得 末端 执行 器 达到 同样 的 位 姿 
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1.3.5 机 器 人 的 瞬 态 运动 学 


1. 瞬 态 运动 学 与 雅 可 比 矩 阵 
正 运动 学 的 内 容 主 要 是 研究 如 何 定 位 末端 执行 器 , 即 得 到 基 坐 标 系 中 如 何 描 述 末 端 执 


行 器 的 位 姿 。 当 机 械 臂 开 始 运 动 时 ,其 中 各 个 关节 上 的 编码 器 会 即时 记录 并 监控 微分 运动 ， 
如 果 在 当前 位 姿 下 把 末端 执行 器 移动 一 段 很 小 的 位 移 , 就 会 得 到 末端 执行 器 一 个 特定 的 
位 姿 。 


即 : 


假设 末端 执行 器 的 位 姿 是 x, 关 节 角 为 0, 则 线 速度 为 ,关节 角速度 为 0 。 
。 对 于 正 向 运动 学 ,需要 解决 的 问题 是 : 07. 
。 对 于 道 运 动 学 ,需要 解决 的 问题 是 : z+ 一 0。 
。 而 对 于 瞬 态 运动 学 : 0 十 60-~z 十 0z ,需要 解决 的 问题 则 是 : 60->67z, 即 从 关节 角速度 
到 线 速度 ; 0 一 会。 
可 以 通过 一 个 矩阵 将 两 者 联系 在 一 起 ,这 个 矩阵 就 是 雅 可 比 矩阵 。 
2. 雅 可 比 和 矩阵 的 定义 及 求法 
机 械 臂 的 位 姿 x 与 关节 变量 g 的 函数 关系 是 : 
z= f(q) (1. 25) 


xi f1(g) 
| ee (1. 26) 
网 f» 
式 (1.26) 两 边 分 别 计 算 其 速度 ,需要 使 用 微分 和 偏 微分 ,并 用 矩阵 形式 表示 : 
9q 9g 9 gn 
xox = |In 992 9 gn Òq cx» (1.27) 


Qf. Bf. 2 0f. 
9q 92q 9 qn ) (nxn 


用 机 械 臂 末 端 在 笛 卡 儿 空间 的 速度 表示 Ò X oco ,用 关节 速度 q 表示 639c6xv， 则 机 械 辟 的 雅 
可 比 矩 阵 定 义 为 机 械 臂 末端 的 笛 卡 儿 速 度 与 关节 速度 的 线性 变换 ,用 公式 表达 如 下 : 


x=J(gq)g (1. 28) 


其 中 ,J(q) 为 机 械 辟 的 雅 可 比 和 矩阵 ,为 一 个 mX n 的 偏 导 数 和 矩阵 。 操 纵 机 器 人 时 ,想得到 微 
分 运动 ex 所 对 应 的 6q ,可 用 下 式 : 


8 一 J(q) 一 六 (1. 29) 
将 末端 执行 器 的 线 速度 (v,,v, «o. FILE HE Ciwa «os «e. XX 6 个 变量 作为 雅 可 比 矩 阵 等 


UR SUEAESMATLADILBSA TRE I> 9 


式 的 左边 , 即 : 


= Jw fwv a. 30) 


上 式 中 ,n 代表 机 器 人 的 自由 度 ,Jo 在 运动 学 中 起 着 非常 重要 的 作用 ,可 以 用 于 对 速度 的 表 
述 。 因 为 所 有 对 速度 的 表述 都 与 线 速度 和 角速度 相关 ,任何 与 速度 有 关 的 表述 都 可 以 和 这 
个 雅 可 比 矩 阵 建 立 联系 。 

将 这 个 雅 可 比 和 矩阵 去 掉 下 标 ( 即 作为 基本 雅 可 比 矩 阵 , 这 个 矩阵 建立 了 线 速度 及 角 
速度 与 关节 角速度 之 间 的 关系 。 根 据 这 种 关系 也 将 雅 可 比 和 矩阵 J 分 为 两 部 分 , 即 : 


Joss I (1.31) 
网 


对 于 矩阵 几 , 即 为 联系 关节 角速度 和 末端 执行 器 线 速度 的 矩阵 。 在 笛 卡 儿 坐 标 中 ,由 
前 面 齐 次 转换 和 矩阵 了 可 知 ,矩阵 了 的 最 后 一 列 中 的 前 三 个 变量 (p;, py，p:) 表 示 末 端 执行 器 
或 者 物体 最 后 一 个 关节 坐标 系 相 对 于 基 座 参考 坐标 系 的 位 置 ,将 这 三 个 变量 统一 表示 为 一 
个 位 置 变量 zw, 则 线 速度 可 以 表示 为 : 


a 
T 。 9r, NEED eo 93, s 
v » X, 2n 14 2d: gz 十 2q, qn (1.32) 
从 中 可 得 : 
9x, 9xp 9r, 
we I- LL (1.33) 
J Ea 9g2 aav ) 


上 式 即 为 与 线性 运动 相关 的 雅 可 比 矩 阵 J,。 当 然 , 从 结构 定义 可 知 , 上 式 是 指 当 关 节 
为 转动 关节 时 的 矩阵 , 当 机 器 人 关节 为 移动 关节 时 ,其 对 应 的 变量 为 0。 
IFE 几 , 即 为 联系 关节 角速度 末端 执行 器 角速度 的 矩阵 。 角 速度 可 以 表示 为 ， 


qi 
e-(hn Emo Bu) a. 34) 
d, 
从 中 可 得 : 
Ju (6m Ea e a) (1.35) 


上 式 中 ,ei 二 1 一 ei, 当 关节 为 转动 关节 时 ,ei 二 0; 当 关 节 为 移动 关节 时 ,ei 二 1。 而 每 个 < 
的 定义 ,对 于 每 个 机 器 人 关节 ,旋转 都 是 指 绕 Z 轴 的 旋转 ,而 x; 则 是 指 齐 次 变换 矩阵 ?T 中 的 
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第 三 列 前 三 个 变量 表示 为 一 个 旋转 变量 , 即 
(ris re ra)" 
分 别 得 到 J, 和 J 之 后 ,最 终 就 得 到 了 需要 的 反映 末端 执行 器 线 速度 和 角速度 的 雅 可 比 
矩阵 。 
在 MATLAB 机 器 人 工具 箱 中 ,可 用 函数 SerialLink. jacob0() 求 出 对 应 某 个 位 姿 下 世 
界 坐标 系 中 的 雅 可 比 矩 阵 , 可 用 函数 SerialLink. jacobn() 求 出 对 应 某 个 位 姿 下 工具 坐标 系 
中 的 雅 可 比 矩 阵 。 


W KUKA KR5 机 器 人 为 例 ,给 定 一 组 关节 角 ,4 一 [0 = pi 0 Bi o ] th ei 


位 姿 下 的 雅 可 比 矩阵 。 
输入 命令 : 


>> mdl KR5 
>> q= [0 pi/4 pi 0 pi/4 0]; 
>> J0 = KR5. jacob0 (q) 


运行 结果 : 
J0= 
一 0.0000 -0.8928 0.4686 0.0000 0.1150 0 
0.0810 一 0.0000 0.0000 0.0813 -0.0000 0 
—0.0000 -0.0990 0.5233  — 0.0000 0.0000 0 


0.0000 0.0000 0.0000 -0.7071 0.0000 -0.0000 
—0.0000 -1.0000 1.0000 0.0000 1.0000 0.0000 
1.0000 0.0000 - 0.0000 0.7071 -0.0000 1.0000 


输入 命令 : 


>>Jn= KR5. jacobn(q) 


运行 结果 
Jn = 
0. 0000 0.8928 -0.4686 -0.0000 -0.1150 0 
一 0.0810 0.0000 0.0000 一 0.0813 0 0 
0.0000 一 0.0990 0.5233 一 0.0000 0 0 
—0.0000 -0.0000 0.0000 0.7071 0 0 
0 


0.0000 1.0000 -— 1.0000 -0.0000 -1.0000 
1.0000 -0.0000 0.0000 0.7071 0.0000 1.0000 


3. 雅 可 比 矩 阵 参 考 坐标 系 的 变换 
已 知 坐标 系 {B} 中 ,6 关 1 的 笛 卡 儿 速 度 矢量 可 以 通过 以 下 变换 得 到 在 坐标 系 {A}) 中 的 
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变换 : 
as ^v 2R Ou 
mn 
CA LAE BR FR CA) 1B} 中 的 雅 可 比 矩 阵 分 别 为 : 
Ax —^J(p d 0.37) 
Py —t"ppq (2.38) 
因此 ,可 以 得 到 雅 可 比 矩 阵 参考 坐标 系 的 变换 ; 
R 0 
0 $R 
以 上 面 的 KUKA KR5 机 器 人 为 例 ,通过 下 列 语句 可 以 得 到 基 坐 标 系 到 工具 坐标 系 的 齐 次 
变换 矩阵 输入 命令 : 


41 (9) = | juco a. 39) 


运行 结果 : 
从 而 可 以 得 到 旋转 矩阵 : 
1. 6- 0 
"=f -—E | 
0 0 —1 


因此 可 得 到 相应 的 变换 矩阵 ,输入 命令 : 


运行 结果 : 
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0.9000 
0.0000 
0 

一 0.0000 
1.0000 


— 0.0000 
0.7200 
0 

—1.0000 

— 0.0000 


— 0.0000 

— 0.1200 
0 
1.0000 

— 0.0000 


ooo 


0 
一 1.0000 


0 

0 

0 

1.0000 

— 0.0000 


ceo 


— 1.0000 


可 以 看 出 ,结果 与 j 的 值 相 同 。 
4. 速度 的 笛 卡 儿 变 换 
当 坐标 系 {A} 和 坐标 系 { 了 B} 是 刚性 连接 时 , 且 {A} 通 过 选择 矩阵 ,再 通过 平移 矢量 得 到 
(B MEE ,通过 下 式 可 以 得 到 速度 在 两 个 坐标 系 中 表示 的 变换 。 
£ ER 一 名 x4Pe 


UB 
n 0 AR 


WB 


^v 


写成 矩阵 形式 为 : 
Bvp =Å T,^ va 
这 里 ,4T, 为 一 个 6X6 的 速度 变换 矩阵 。 
TE MATLAB 机 器 人 工具 箱 中 ,可 用 函数 tr2jac() 求 出 不 同 变换 的 雅 可 比 矩 阵 。 例 如 ， 
{B} 是 通过 {A} 平 移 (2,4,0), 再 旋转 45" 得 到 的 , 求 速度 变换 矩阵 T... {AP X Jr Ip ng 2x 
速度 为 2m/s, 求 {B} 中 的 速度 。 
输入 命令 : 


>> T= trans1(2,4,0) * troty(pi/4); 
>> Tv = tr2jac(T) 


运行 结果 
nu 
0.7071 0 -—0.7071 .-—2.8284 1.4142  - 2.8284 
0 1.0000 0 0 0 2.0000 
0.7071 0 0.7071 2.8284 -1.4142 -2.8284 
0 0 0 0.7071 Qj. cet 
0 0 0 0 1.0000 0 
0 0 0 0.7071 0 0.7071 
输入 命令 : 
2»vB- Tv« [2000 0 0]'; 
2» yB' 
运行 结果 
ans = 
1.4142 0 1.4142 0 0 0 
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可 以 得 到 ,该 速度 在 {B} 中 X 方向 为 1.4142m/s, 在 乙方 向 速度 为 1. 4142m/s. 
1.3.6 具体 例子 的 应 用 


【 例 1-4】 一 只 三 连 杆 的 平面 机 器 人 如 图 1-25 所 
R: 参考 坐标 系 0 代表 世界 坐标 系 ,参考 坐标 系 i= 
1,2,3) 是 一 个 与 连 杆 i 相关 的 坐标 系 , 连 杆 的 长 度 分 别 
Æ h.n. 

(1) 获取 相 邻 连 杆 之 间 的 参考 坐标 系 变换 , 即 1T， 
TST ENET 0.0; 8 0; 的 函数 。 

(2) 获取 世界 参考 系 中 的 连 杆 和 机 器 人 手中 的 参考 


系 之 间 的 变换 。 
(3) 假设 连 杆 的 长 度 h= l= h HRILA FE 
ms 0 0 0 
=i 0 1 -25 三 连 ; 
到 由 | 。 | 1 给 出 的 相对 应 的 世界 参考 系 的 图 5 三 和 本 的 平面 机 器 人 
0 0.0 1 


位 置 ,尝试 获取 0.,0, 和 0 的 值 。 

(4) 证 明 : 一 般 来 说 ,这 个 机 器 人 的 逆 运 动 学 问题 有 两 个 解决 方案 ,并 在 MATLAB 中 
写 一 个 函数 ,返回 两 种 方案 的 结果 。 

SI : 

(1) 相关 代码 如 下 : 


al = sym('al'); 
l1 = syn('11"); 
a2 = syn('a2"); 
12 = syn('12"); 
a3 = syn('a3"); 
13 = syn('13"); 
T01 = trotz(al) * transl(ll, 0, 0); 
T12 = trotz(a2) * trans1(12, 0, 0); 
T23 = trotz(a3) * trans1(13, 0, 0); 


(2) 相关 代码 如 下 : 


T03 = T01% T12 x T23; 

名 多 旬 或 者 ,我 们 也 可 以 使 用 其 D 参数 来 解决 该 机 器 人 的 正 向 运动 学 . 
sssi Wais 1 

L(1) = Link([00 1 0]); 

L(2) = Link([0 0 1 0]); 
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L(3) = Link([0 0 1 0]); 

ThreeLink = SerialLink(L); 

ThreeLink.name - 'Planar3R'; 

名 名 名 为 了 验证 我 们 已 经 正确 地 构建 了 机 器 人 ,我 们 将 它 绘制 出 来 . 
ThreeLink.plot([pi/4 pi/4 pi/4]); 

ThreeLink. fkine( [pi/4 pi/4 pi/4]); 


运行 结果 如 图 1-26 Bros 。 
(3) 相关 代码 如 下 : 


TL=[-1000;0-101;0010;0001]; 

名 多 名 一 种 可 能 性 在 于 使 用 由 这 ine 实 现 的 数值 方法 . 问题 是 给 出 一 个 好 的 猜测 ， 
名 名 名 以 便 算法 收 全 到 一 个 有 效 的 解决 方案 .在 一 些 试验 和 错误 之 后 ,良好 的 起 
% % % ije theta 1 = 0,theta 2 = pi,theta 3 =x,Q0 = [0 -pi/2 pi/2]; 
QF = ThreeLink.ikine(TL, Q0, [110001]); 

ThreeLirk. plot(QF); 

* xs 我 们 可 以 获得 QO 和 OF 之 间 的 轨迹 

TRAJ = jtraj(Q0, QF, (0:.05:1)); 

ThreeLink. plot( TRAJ); 


运行 上 面 的 代码 ,得 到 机 器 人 轨迹 ,如 图 1-27 所 示 。 


2 
1.5 1 
1 
0.5 
0 | 
-0.5 
-l 
-15 
-2 上 二 上 上 上 
5 10 15 20 
图 1-26 使 用 工具 箱 生成 的 平面 机 器 人 图 1-27 机 器 人 运行 的 轨迹 


OD 相关 代码 如 下 : 


function theta = ikine3r(l, conf, sigma) 

名 % * IKINE3R 解决 了 3R F IB Blas A H3 35 z^ [THETA] THETA2 THETA3] = ikine3r 
5 % 5 ([111,123], [XY PHI],SIGMA) 返 回 与 围绕 三 个 关节 的 旋转 对 应 的 3 个 角度 

名 名 名 的 向 量 . sigma = +/- 1 是 一 个 标志 ,给 我 们 两 个 可 能 的 解决 方案 之 一 

xx = conf(1)— 1(3) * cos(conf(3)); 

YY = conf(2) 一 1(3) * sin(conf(3)); 


u 


12 
D 
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theta(1)- atan2(yy,xx) +... 

signa x acos((1(1) x 1(1) * xx x xx+ yy * yy- 1(2) * 1(2))/... 
(2* 1(1) * sqrt(xx * xx * yy * yy))); 
theta(2) = atan2( yy - 1(1) * sin(theta(1)),... 

xx- 1(1) * cos(theta(1))) - theta(1); 

theta(3) = conf(3) - theta(1) - theta(2); 

end 


1.3.7 机 器 人 工具 箱 的 Link 类 

根据 Robotics Toolbox for MATLAB ,对 机 器 人 工具 箱 中 的 Link 类 进行 进一步 的 
解析 。 

机 器 人 工具 箱 中 的 Link 对 象 保存 与 机 器 人 连 杆 相关 的 所 有 信息 ,例如 运动 学 参数 、 刚 
体 惯性 参数 .电机 和 传动 参数 等 。 

与 Link 对 象 有 关 参 数 如 下 所 示 : 


参数 意义 b W E x 

k 连 杆 变换 矩阵 islimit 测试 关节 是 否 超 过 软 限制 
RP RP 关节 类 型 : 'R' 或 'P' isrevolute 测试 关节 是 否 旋转 关节 
friction 摩擦 力 isprismatic 测试 关节 是 否 移动 关节 
nofriction 将 摩擦 参数 设置 为 零 的 连 杆 对 象 display 以 可 读 的 形式 打印 连 杆 参数 
dyn 显示 连 杆 动态 参数 char 转换 为 字符 串 


写 人 / 读 取 Link 的 参数 如 下 所 示 : 


2 数 意 x 2 * 意 义 
theta | 运动 学 : 关节 角度 m 动力 学 : 连 杆 质量 

d 运动 学 : 连 杆 偏 移 r 动力 学 : 连 杆 的 重心 3X1 

a 运动 学 : 连 杆 长 度 I 动力 学 : 连 杆 的 惯性 矩阵 3X3 
alpha | 运动 学 : 连 杆 扭转 角 B 动力 学 : 连 杆 黏 性 摩擦 (电机 参考 》 
sigma | 运动 学 : 0 表示 旋转 ,1 表示 移动 Tc 动力 学 : 连 杆 的 库仑 摩擦 
mdh | 运动 学 : 0 表示 标准 DH, 其 他 情况 为 1 g 执行 器 : 齿轮 比 
offset | 运动 学 : 关节 变量 偏 移 Jm 执行 器 : 电机 惯量 (电机 参考 ) 
qim | 运动 学 : 关节 变量 极限 Lmin max] 


注意 : 

(1) 连 杆 对 象 是 一 个 引用 类 对 象 。 

(2) 连 杆 对 象 可 以 在 向 量 和 数组 中 使 用 。 

1. Link; 创建 机 器 人 的 连 杆 对 象 

这 是 具有 几 个 调用 名 的 类 构造 函数 . 它 有 三 种 形式 : 
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(OD L = Link() 是 具有 默认 参数 的 Link 对 象 ; 
(2) L = Link(lnk) 是 一 个 Link 对 象 , 它 是 连 杆 对 象 link 的 副本 ; 
(3) LL 三 Link(options) 是 指定 了 运动 和 动态 参数 的 连 杆 对 象 。 


它 的 参数 如 下 所 示 : 
5 u E x 5 数 € x 
Options 'theta', | 关节 角度 ,如果 没 有 指定 , 则 默认 的 
"RE 求 出 重心 (3X1 阵 ) 
TH 关节 是 旋转 关节 i m dd 
关节 延伸 ,如 果 没 有 指定 , 则 默认 的 
td’; G'G 设置 (默认 值 1) 
d, D Ping 设置 电机 齿轮 比 (默认 人 
"aV A 关节 偏 移 ( 默 认为 0) 'B', B | 关节 摩擦 (默认 为 0) 
alpha, A | 关节 的 扭转 角 ( 默 认为 0 Jm.) | 电机 惯量 (默认 为 0) 
1X1 2X1) 认 值 
'standard — 使 用 标准 型 DH 参数 法 (上 默认) 定义 ‖ "Ter. T iria 或 aus 
'modified" 使 用 修改 的 DH 参数 定义 'revolute' 设置 为 旋转 关节 (默认 ) 
'offset', O 关节 变量 offset( 默 认为 0) 'prismatic | 设置 移动 关节 'p' 
"im. 1 | 关节 限制 (默认 为 []) mo M | 与 连 杆 的 质量 相关 
Tug 连 杆 惯 性 乱 阵 (3X1,6X1 或 3X3| ,sym， | 将 所 有 参数 值 视 为 符号 而 非 数字 
的 矩阵 
注意 : 
(OD 同时 指定 'theta' 和 'd' 是 错误 的 ; 


(2) 连 杆 的 惯性 矩阵 (3X3) 是 对 称 的 ,可 以 通过 给 定 一 个 3X3 的 矩阵 (对 角 元 素 [Ixx 


Iyy Izz D 3,48 FE 84 4E fo k dn [ Ixx lyy Izz Ixy Iyz Ixz] 来 指定 


(3) 所 有 摩擦 量 均 以 电机 而 不 是 负载 为 参考 ; 

OD 齿轮 比 仅 用 于 转换 电机 参考 量 。 

2. Link. A: 连 杆 的 变换 矩阵 

T — L. AGp : 求 对 应 于 连 杆 变量 q 的 连 杆 均匀 变换 矩阵 (4X4) , 连 杆 变量 9 是 DH 参 


数 THETA( 旋 转 ) 或 D( 移 动 ) 。 


注意 : 

CD 对 于 旋转 关节 ,忽略 连 杆 的 THETA 参数 ,而 改 为 使 用 gq。 

(2) 对 于 移动 关节 ,忽略 连 杆 的 D 参数 ,而 改 为 使 用 dg。 

(3) 在 计算 变换 答 阵 之 前 ,将 连 杆 偏 移 参数 添加 到 g。 

(4) Link. char; 转换 为 字符 串 。 

s = L.charO: 是 一 个 以 紧凑 单行 格式 显示 连 杆 参数 的 字符 串 。 如 果 LL 是 一 个 Link 


对 象 的 向 量 , 则 返回 每 个 连 杆 一 行 的 字符 串 。 


3. Link. display: 显示 连 杆 的 参数 
L. display() 以 紧凑 单行 格式 显示 连 杆 参数 。 如 果 L 是 连 杆 对 象 的 向 量 , 则 每 个 元 素 
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显示 一 行 。 

注意 : 当 表达 式 的 结果 是 Link 对 象 并 且 命 令 没 有 尾部 分 号 时 ,在 命令 行 中 隐 式 调用 此 

4. Link. dyn: 显示 连 杆 的 惯性 属性 

L. dyn() 以 多 行 格式 显示 连 杆 对 象 的 惯性 属性 。 属 性 包括 质量 、 质 心 ,惯性 、 摩 擦 、 传 动 
比 和 电机 性 能 。 如 果 L 是 连 杆 对 象 的 向 量 , 则 显示 每 个 连 杆 的 属性 。 

5. Link. friction: 关节 摩擦 力 

f = L. friction(qd) 是 用 于 连 杆 速度 gd 的 关节 摩擦 力 /扭矩 。 

注意 : 

(1) 1 返回 的 摩擦 值 被 称 为 齿轮 箱 的 输出 ; 

(2) Link 对 象 中 的 摩擦 参数 以 电机 为 参考 ; 

G) 电机 黏 性 摩擦 力 按 G2 放大 ; 

OD 电机 库仑 摩擦 由 G 放大 ; 

(5) 在 非 对 称 情况 下 使 用 的 适当 的 库仑 摩擦 值 取决 于 对 关节 速度 的 符号 ,而 不 是 电机 
速度 。 

6. Link. islimit; 测试 关节 极限 

如 果 q 超出 为 该 关节 设置 的 软 限制 , 则 L.islimit(q) 为 真 ( 即 为 1) 。 

注意 : 任何 工具 箱 函 数目 前 都 不 使 用 这 些 限制 。 

7. Link.isprismatic: 测试 关节 是 否 为 移动 关节 

如 果 关 节 是 移动 的 , 则 L.isprismatic() 为 1; 如 果 关 节 是 转动 的 , 则 L. isprismatic() 
为 0。 

8. Link.isrevolute: 测试 关节 是 否 旋转 

如 果 关 节 是 转动 的 , 则 L.isrevolute() 为 1; 如 果 关 节 是 移动 的 , 则 L.isrevolute() 为 0。 

9. Link.issym: 检查 连 杆 是 否 是 符号 模型 

如 果 连 杆 L 具有 符号 参数 , 则 res = L.issym( ) 为 真 。 

10. Link.nofriction: 清除 连 杆 的 摩擦 

In = L. nofriction() 是 除了 非 线 性 之 外 具有 与 L 相同 的 参数 的 连 杆 对 象 , 使 得 它 的 ( 库 
仑 ) 摩 擦 系数 为 零 。 

(D ln = L. nofriction('all') ,除了 斐 性 和 库仑 摩擦 ,其 他 的 参数 设置 为 零 。 

(2) ln = L. nofriction('coulomb 0 ,除了 库仑 摩擦 ,其 他 的 参数 设置 为 零 。 

(3) ln = L., nofriction('viscous') .除了 黏 性 摩 掠 ,其 他 的 参数 设置 为 零 。 

注意 : 有 限 库 仑 摩擦 的 前 向 动力 学 仿真 可 能 很 慢 。 

11. Link. RP; 获取 关节 类 型 

c = L. RPO 〇 是 一 个 字符 “R” 或 “P”, 取 决 于 关节 是 旋转 还 是 移动 的 。 如 果 工 是 一 个 
Link 对 象 的 向 量 , 则 以 联合 顺序 返回 一 个 字符 串 。 


48 4| 机 器 人 仿真 与 编程 技术 


12. 三 项 参数 的 设置 

1) Link. set. I; 设置 连 杆 惯量 

L. I — [Ixx Iyy Izz] 将 连 杆 惯量 设置 为 对 角 和 矩阵 。 

L. I = [Ixx Iyy Izz Ixy Iyz Ixzj 将 连 杆 惯量 设置 为 具有 指定 惯性 和 惯性 元 素 乘 积 的 对 
FRIERE 

L I— M 设置 连 杆 的 惯性 矩阵 为 MOX 3) , 它 必 须 是 对 称 的 。 

2) Link. set. r: 设置 重心 

L. r= 有 将 连 杆 重心 (COG) 设 置 为 R(3- 矢 量 ) 。 

3) Link. set. Tc; 设置 库仑 摩擦 

L. Tc = FECE ASUEBN[F 一 F], 对 于 模型 的 对 称 库仑 摩擦 。 

L. Tc = [FP FM] 将 库仑 摩擦 设置 为 [FP FM], 用 于 非 对 称 库仑 摩擦 模型 。FP> 0 和 
FM 一 0。 其 中 ,FP 用 于 正 关节 速度 ,FM 用 于 负 关 节 速 度 。 

注意 : 摩擦 参数 被 定义 为 对 于 正 关节 速度 是 正 的 ,通过 Link. friction 计算 的 摩擦 力 使 
用 摩擦 参数 的 负数 , 即 , 与 关节 的 运动 相反 的 力 。 


1.3.8 机 器 人 工具 箱 的 SerialLink 类 1 


根据 Robotics Toolbox for MATLAB ,本 节 将 对 SerialLink 类 涉及 机 器 人 运动 学 内 容 
进行 进一步 的 解析 。 

机 器 人 工具 箱 中 的 SerialLink 类 表示 串联 臂 型 机 器 人 的 具体 类 。 该 机 制 使 用 DH 参数 
描述 ,每 个 关节 一 组 。 

SerialLink 类 包含 的 参数 如 下 所 示 : 


5 数 E Ox 参 数 意义 参 数 m ox 
plot victi ad jacobo Fi uM me PODEA 
plot3d UM LU en re fdyn 正 向 动力 学 
teach i ue Jacob dot | 雅 可 比 衍生 物 jiead i m 
— a — 

getpos I a maniplty | 可 操作 性 perturb aae ERN 
jtraj 关节 空间 轨迹 vellipse | 显示 速度 椭圆 体 。 |eraviac | 重力 荷载 和 雅 可 比 
edit STT ANENA fellipse | SE JU paycap | 有 效 载荷 能 力 

态 参数 
isspherical S 70 qmincon E Cel oeil pay 有 效 载荷 效应 
islimit Doe acd | 关节 加 速 syin 对 象 的 符号 版 本 

节 


第 1 章 ”机 器 人 学 与 MATLAB 机 器 人 工具 箱 |» 49 


EI 
2 数 意 x $ 数 意 x $ * E& x 
isconfig | 测试 机 器 人 关节 配置 | coriolis 科 里 奥 利 关节 力 gencoords | 符号 广义 坐标 
fkine 正 向 运动 学 dyn 显示 连 杆 的 动态 属性 | genforces | 符号 广义 力量 
PEMEX 3 r 

trchain Lom F [friction | 摩擦 力 ikine sym ae c 
m 6 轴 球 形 手腕 旋转 机 测试 对 象 是 否 是 符 
ikine6s 器 人 的 道 运动 学 gravload | 重力 关节 力 issym 号 的 
die | 饮用 挝 代数 什 方 法 的 |。 | ER A 连 杆 的 变换 矩阵 

道 运动 学 
ikunc RRHLILO IE Ios cinertia fü FJLTR PER IE 

动 学 
ikcon 使 用 关节 限制 优化 的 nofriction | 将 摩擦 参数 设置 为 零 

道 运动 学 


写 入 / 读 取 SerialLink 类 的 参数 如 下 所 示 : 


参 数 意 x 

links 连 杆 对 象 的 向 量 (1XN 的 矩阵 
gravity | 重力 方向 [gx gy gz] 

base 机 器 人 基础 姿势 (4X4 的 矩阵) 
机 器 人 的 工具 变换 ,T6 到 工具 的 
末端 (4X4 的 矩阵 ? 

关节 极限 ,[qmin qmax] ON X 2 的 


2 数 意 x 

comment | 注释 ,一 般 性 注释 

plotopt | plot() 方 法 的 选项 (单元 数组 ) 

使 用 MEX 版 本 的 RNE。 只 有 在 mex 文件 
存在 时 才能 设置 为 true。 默 认 值 为 true。 


n 关节 数量 (只 读 ) 


fast 


tool 


qlim config 关节 配置 字符 串 ,例如 。'“RRRRRR( 只 读 ) 


和 矩阵) 

运动 学 关节 坐标 偏 移 CN Xx 1 的 矩 运动 学 约定 布尔 (0 = DH,1 = MDH) (只 
offset mdh 4 

H) 读 ) 
name 机 器 人 的 名 称 , 用 于 图 形 显 示 theta 运动 学 : 关节 角度 (1X N 的 矩阵)( 只 读 ) 
manuf 注释 ,作者 的 注释 d 运动 学 : 连 杆 偏 移 (1X N 的 矩阵 )( 只 读 ) 
a 运动 学 : 连 杆 长 度 (1XN 的 矩阵 ) | alpha 运动 学 : 连 杆 扭 角 (1X N 的 和 矩阵)( 只 读 ) 

注意 : 


(1) SerialLink 是 一 个 引用 对 象 。 

(2) SerialLink 对 象 可 以 在 向 量 和 数组 中 使 用 。 

1. SerialLink: 创建 一 个 SerialLink 机 器 人 对 象 

(OD R = SerialLink(links,options) 是 一 个 由 Link 类 对 象 定义 的 机 器 人 对 象 , 它 可 以 
是 Link. Revolute. Prismatic. RevoluteMDH 或 PrismaticMDH 的 实例 。 

(2) R — SerialLink(options) 是 一 个 没有 连 杆 的 空 机 器 人 对 象 。 

(3) R = SerialLink([ R1 R2 …], 选 项) 连接 机 器 人 ,R2 的 基 座 连接 到 R1 的 末端 。 也 
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可 以 写成 R1 x R2 等 。 
(4) R = SerialLink(R1 ,选项 ) 是 机 器 人 对 象 R1 的 深层 副本 ,具有 所 有 相同 的 属性 。 
(5) R = SerialLink(dh,options) 是 具有 由 矩阵 dh 定义 的 运动 学 的 机 器 人 对 象 ,其 中 

每 个 关节 具有 一 行 ,并 且 每 一 行 是 0a 并 且 假 设 关 节 被 旋转 。 可 选 的 第 五 列 sigma 指示 旋转 

(sigma = 0, 默 认 ) 或 移动 (sigma = D. 


它 的 参数 如 下 所 示 : 
参 数 意 5E E 数 意 A 
将 机 器 人 名 称 属 性 设置 "e : J 、 

name' ,NAME 为 NAME gravity',G 设置 重力 矢量 属性 为 G 
'comment', | 将 机 器 人 注释 属性 设置 | aiotopt,P | 将 plotG 的 默认 选项 设置 为 P 
COMMENT 为 COMMENT 
'manufacturer', | 设置 机 器 人 制造 商 属 性 |， oa 将 . plot3d() 的 默认 选项 设置 
MANUF 为 MANUF ETTORRA 为 P 
'base', T 设置 基底 变换 矩阵 属性 为 T "nofast" 不 要 使 用 RNE MEX 文件 
'tool', T 设置 工具 转换 矩阵 属性 为 T 


实例 : 
1) 创建 一 个 两 连 杆 的 机 器 人 


L(2) 


L(1) = Link([ 0 0 a1 pi/2], 'standard'); 
= Link([ 0 0 a2 0], 'standard'); 


twolink = SerialLink(L, 'name', 'two link'); 


2) 创建 一 个 两 连 杆 的 机 器 人 (最 具 描 述 性 ) 


L(1) = Revolute('d', 0, 'a', a1, 'alpha', pi/2); 
L(2) = Revolute('d', O, 'a', a2, 'alpha', 0); 
twolink = SerialLink(L, 'name', 'two link'); 


30 创建 一 个 两 连 杆 的 机 器 人 (最 少 描述 性 ) 


twolink = SerialLink([0 0 al 0; 00 a2 0], 'name', 'two link'); 


4) 机 器 人 对 象 可 以 以 两 种 方式 连接 


R= RI * R2; 
R = SerialLink([Rl R2]); 
注意 : 


(D SerialLink 是 一 个 引用 对 象 ,一 个 Handle 对 象 的 子 类 。 
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(2) SerialLink 对 象 可 以 在 向 量 和 数组 中 使 用 。 

(3) 传 入 的 连 杆 子 类 元 素 必须 是 所 有 标准 或 所 有 修改 的 DH 参数 。 

(4) 当 机 器 人 连接 时 ,中 间 基 础 和 工具 变换 被 删除 ,因为 通常 的 常数 变换 不 能 以 DH 符 
号 表示 。 

2. SerialLink.A: 连 杆 的 变换 矩阵 

(OD s 三 R. A(U,qj) 是 从 连 杆 帧 人 -lg 变换 到 作为 第 J 个 联合 变量 aj 的 函数 的 帧 Jg 的 
SE(3) 均 匀 变 换 (4X4)。 

(2) s = R. AGjlist,q) ,但 是 是 在 列表 JLIST 中 给 出 的 连 杆 变换 矩阵 的 组 合 , 并 且 联 合 
ARBORE Q 的 相应 元 素 。 

3. SerialLink. accel 机 械 臂 正 向 动力 学 

(D qdd = R. accel(q,qd,torque) 是 在 状态 q 和 qd 下 向 驱动 器 力 / 转 矩 施加 到 机 械 手 
机 器 人 R 而 产生 的 关节 加 速度 的 矢量 (N 宇 1) ,N 是 机 器 人 的 关节 数量 。 

如 果 q,qd, 转 矩 是 矩阵 (K 宇 N), 则 qdd 是 矩阵 (KE ND ,其 中 每 行 对 应 于 q,qd, 转 矩 的 
等 效 行 的 加 速度 。 

(2) qdd = R. accel(x) 如 上 所 述 ,但 是 x = [q,qd, 转 矩 ](1X3N) 。 

注意 : 

(1) 用 于 仿真 机 械 手 动力 学 ,结合 数值 积分 函数 。 

(2) 使 用 Walker 和 Orin 的 方法 1 计算 正 向 动力 学 。 

(3) Featherstone 的 方法 对 于 具有 大 量 关节 的 机 器 人 更 有 效 。 

(4) 应 当 考虑 关节 摩擦 。 

4. SerialLink. animate; 更 新 机 器 人 动画 

Ranimate(q) 更 新 机 器 人 R 的 现 有 动画 。 这 将 使 用 R. plot() 创 建 。 在 所 有 图 中 更 新 
此 机 器 人 的 图 形 实例 。 

注意 : 

A) 由 plot() 和 plot3d() 调 用 来 实际 移动 手臂 模型 。 

(2) 用 于 Simulink 机 器 人 动画 。 

5. SerialLink. char: 转换 为 字符 串 

s = R.char() 是 机 器 人 运动 参数 的 字符 串 表 示 ,显示 DH 参数 .关节 结构 .注释 .重力 
矢量 .基准 和 工具 变换 。 

6. SerialLink. editO : 编辑 SerialLink 机 械 臂 的 运动 参数 和 动态 参数 

(1) R. edit 在 新 的 图 形 界 面 中 将 机 器 人 的 运动 参数 显示 为 可 编辑 的 表 。 

(2) R. edit('dyn') 如 上 所 述 ,但 也 显示 动态 参数 。 

注意 : 

(1)“ 保 存 ” 按 钮 将 值 从 表 中 复制 到 SerialLink 操纵 器 对 象 。 

(2) 要 退出 编辑 器 而 不 更 新 对 象 , 只 需 杀 死 图 形 窗口 。 


Du 
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7. SerialLink. fkine: 用 于 计算 机 器 人 的 正 向 运动 学 

(D T= 二 R. fkine(q,options) 是 机 器 人 末端 执行 器 的 姿态 ,作为 关节 配置 qa(1XN) 的 
SE(3) 均 匀 变 换 (4X4)。 

如 果 q 是 矩阵 (KE ND , 则 行 被 解释 为 沿 着 轨迹 的 点 序列 的 广义 联合 坐标 。q(i,j) 是 第 i 
个 轨迹 点 的 第 j 个 联合 参数 。 在 这 种 情况 下 ,T 是 一 个 3d 矩阵 (4X4XK), 其 中 最 后 一 个 
下 标 是 沿 着 路 径 的 索引 。 

(2) [T,all] = R. fkine(q) ,但 是 所 有 的 (4X4XN) 是 连 杆 的 坐标 系 1 到 N 的 姿态 k。 

8. SerialLink. ikcon 

D 具有 关节 限制 的 数值 逆 运 动 学 

q = R.ikcon(T) 是 对 应 于 作为 均匀 变换 的 机 器 人 末端 执行 器 姿态 T(4X4) 的 关节 坐 
标 (1XN)。 

[q.err] = robot. ikcon(T) 如 上 所 述 ,但 也 返回 err, 它 是 目标 函数 的 标量 最 终 值 。 

[q,err,exitflag] = robot. ikcon(T) ,但 也 返回 fmincon 的 状态 exitflag。 

[q.err.exitflag] = robot.ikcon(T,q0), 但 是 指定 用 于 最 小 化 的 初始 关节 坐标 q0 。 

[q.err.exitflag] = robot. ikcon( T., q0, options) 如 上 所 述 , 但 指定 fmincon 使 用 的 
选项 。 

2) 轨迹 操作 

在 所 有 情况 下 ,如 果 工 是 4X4 关 M 和 矩阵 作为 均匀 变换 序列 ,并 且 R.ikcon() 返 回 与 序 
列 中 的 每 个 变换 对 应 的 联合 坐标 。g 是 MXN 矩阵 ,其 中 N 是 机 器 人 关节 的 数量 。 对 于 每 
个 时 间 步 长 的 q 的 初始 估计 被 取 作 来 自 先前 时 间 步 长 的 解 。err 和 exitflag 也 是 MXN XE 
阵 , 并 且 指 示 对 应 的 轨迹 步 长 的 优化 结果 。 

iE: 

(1) 需要 优化 工具 箱 中 的 fmincon 。 

(2) 在 本 解决 方案 中 考虑 联合 限制 。 

(3) 可 用 于 具有 任意 自由 度 的 机 器 人 。 

(4) 在 多 个 可 行 解 的 情况 下 ,返回 的 解 取决 于 q0 的 初始 选择 。 

(5) 通过 最 小 化 关节 角度 解 的 正 向 运动 学 和 末端 效应 器 框架 之 间 的 误差 作为 优化 工 
作 。 目 标 函 数 ( 误 差 ) 描 述 为 : 


sunsqr((inv(T) * robot.fkine(q) - eye(4)) * omega) 


9. SerialLink. ikine 

D 数值 逆 运 动 学 

q = R. kinine(T) 是 与 作为 均匀 变换 的 机 器 人 末端 执行 器 姿势 T(4 关 4) 对 应 的 关节 坐 
AXN). 

q = R.ikine(T,q0,options) 指 定 关 节 坐 标的 初始 估计 。 该 方法 可 用 于 具有 6 个 或 更 
多 自由 度 的 机 器 人 。 
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2) 欠 驱 动机 器 人 

对 于 操纵 器 具有 少 于 6 个 DOF 的 情况 . 解 空间 具有 比 可 以 由 操纵 器 关节 坐标 跨越 的 更 
多 的 尺寸 。 

q = R. ikine(T. q0. m. options) ,但 是 其 中 m 是 向 量 (1 6 4B AO ,其 指定 在 达到 解 中 
将 被 忽略 的 笛 卡 儿 DOF( 在 腕 部 坐标 系 中 )。 向 量 m 具有 对 应 于 平移 的 6 个 元 素 。 

fr X.Y 和 2 轴 中 ,以 及 分 别 绕 X,Y 和 Z 轴 旋 转 。 该 值 应 为 0( 对 于 忽略 ) 或 1。 非 零 元 
素 的 数量 应 等 于 操纵 器 DOF 的 数量 。 

例如 , 当 使 用 3 自由 度 机 械 手 时 ,旋转 方向 可 能 不 重要 ,在 这 种 情况 下 mm 三 [11100 
0]。 对 于 具有 4 或 5 自由 度 的 机 器 人 ,这 种 方法 是 非常 难以 使 用 的 ,因为 方向 由 世界 坐标 中 
的 全 指定 ,并 且 可 实现 的 取向 是 刀具 位 置 的 函数 。 

3) 轨迹 操作 

在 所 有 情况 下 ,如 果 了 是 4X4Xm 作为 均匀 变换 序列 ,并 且 R.ikine() 返 回 与 序列 中 的 
每 个 变换 对 应 的 联合 坐标 。4 是 mwX1XN, 其 中 NN 是 机 器 人 关节 的 数量 。 对 于 每 个 时 间 步 
KAY q 的 初始 估计 被 取 作 来 自 先前 时 间 步 长 的 解 。 


1.4 机 器 人 动力 学 


1.4.1 机 器 人 动力 学 概述 


动力 学 主要 研究 产生 运动 所 需要 的 力 。 对 于 机 器 人 动力 学 分 析 , 有 两 种 经 典 的 方法 : 
一 种 是 牛顿 - 欧 拉 法 , 另 一 种 是 拉 格 朗 日 法 。 与 机 器 人 运动 学 相似 ,机 器 人 动力 学 也 有 两 个 
相反 的 问题 : 

(1) 动力 学 正 问 题 是 已 知 机 械 辟 各 关节 的 作用 力 或 力矩 , 求 各 关节 的 位 移 、 速 度 和 加 速 
度 , 即 机 器 人 的 运动 轨迹 (r=>gq,9q «q) ,这 可 以 用 于 对 机 械 臂 的 仿真 。 

(2) 动力 学 逆 问 题 是 已 知 机 械 辟 的 运动 轨迹 , 即 各 关节 的 位 移 、 速 度 和 加 速度 , 求 各 关 
节 所 需要 的 驱动 力 或 力矩 (9,9 ,gz), 这 可 以 用 于 对 机 械 臂 的 控制 。 


1.4.2 机 器 人 动力 学 方程 的 建立 方法 


1. 机 器 人 刚体 的 加 速度 

设 两 个 相互 独立 的 坐标 系 {A} 和 {B} ,坐标 系 {B} 固 连 在 一 个 刚体 上 ,刚体 有 一 个 相对 
于 坐标 系 {A} 的 运动 *2 。 坐 标 系 {B} 相 对 于 坐标 系 {1A} 的 位 置 可 以 用 位 置 矢量 ^Paose 和 旋 
转 矩 阵 争 来 描述 。 则 Q 点 在 坐标 系 {A} 中 的 线 速度 可 以 表示 为 : 

AVo =^V Borce HR” yo (1. 40) 

注意 ,上 式 只 适用 于 在 坐标 系 {A} 和 1{B) 相 对 方位 保持 一 定 的 前 提 下 。 

在 一 般 情 况 下 , 即 机 器 人 均 是 转动 关节 的 时 候 ,Q 点 在 {B} 坐 标 系 中 的 位 置 固定 , 即 "@ 
为 常量 的 时 候 ,关节 转动 时 坐标 系 {B} 相 对 于 坐标 系 {A} 的 旋转 的 角速度 为 ^2s。 经 过 推导 
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和 计算 ,最 后 得 到 机 器 人 的 线 加 速度 的 表达 式 为 : 
4Va 一 ^Vaoge "Qn X (Qn XSR"Q)4-^05 XRQ (1.41) 
通常 情况 下 ,上 式 用 于 计算 转动 关节 机 械 臂 连 杆 的 线 加 速度 。 
同上 假设 ,关节 转动 时 坐标 系 { 了 B} 相 对 于 坐标 系 {A} 旋 转 的 角速度 为 2 ,而 坐标 系 {1C} 
相对 于 坐标 系 {B} 旋 转 的 角速度 为 >?Qc, 则 坐标 系 {C} 相 对 于 {A} 旋 转 的 角速度 为 : 
^ne —^05 -SR"Qc (1. 42) 
然后 对 其 求 导 ,最 终 得 到 : 
^c 一 40s 十 8Ra0De 十 Da XsR'Qc (1.43) 
由 上 式 即 可 计算 机 械 臂 连 杆 的 角 加 速度 。 
2. 机 器 人 刚体 的 质量 分 布 
分 析 机 器 人 动力 学 的 时 候 还 应 考虑 到 机 器 人 刚体 的 质量 分 布 。 对 于 转动 关节 机 械 辟 
( 定 轴 转 动 ) ,在 一 个 刚体 绕 任意 轴 做 旋转 运动 的 时 候 , 用 惯性 张 量 表 示 机 器 人 刚体 的 质量 分 
布 。 在 基于 刚体 构建 的 坐标 系 , 如 坐标 系 1A} 上 的 惯性 张 量 可 表示 为 : 


| =k =k 
41 = | 一 Ts I, —l, (1. 44) 
—I& —l, Iz 
其 中 的 各 元 素 分 别 为 : 
Ix = |||? +=?odv (1. 45) 
Y 
Iy = [|a + z!)pdv (1. 46) 
"=y 
L, = || G* + »Dpdo (1.47) 
y 
Ix = IEZ (1. 48) 
V 
I, = IEZ (1. 49) 
v 
了 一 IEZ (1.50) 
Ld 


上 式 中 ,机 器 人 刚体 由 微分 体积 dv 组 成 ,其 密度 是 p, 其 中 每 个 微分 体 的 位 置 由 其 坐标 确 
定 , 而 上 面 6 个 相互 独立 的 元 素 的 大 小 取决 于 所 在 坐标 系 的 位 姿 。 其 中 ,1 、1yy 和 1 称 为 惯 
HERE ,其余 3 个 称 为 惯量 积 , 参 考 坐 标 系 的 轴 称 为 主轴 。 

3. 牛顿 - 欧 拉 递 推动 力学 方程 

将 机 器 人 上 的 连 杆 看 作 刚体 .首先 应 确定 机 器 人 每 个 连 杆 的 质量 分 布 (包括 质心 位 置 和 
惯性 张 量 ) ,这 是 控制 连 杆 进行 加 速 和 减速 运动 的 前 提 , 即 连 杆 运动 所 需 的 驱动 力 是 关于 连 
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杆 的 期 望 加 速度 和 质量 分 布 的 函数 。 而 牛顿 - 欧 拉 方程 就 描述 了 力 或 力矩 与 惯量 、 加 速度 
等 之 间 的 关系 。 
根据 牛顿 第 二 定律 , 即 物体 加 速度 的 大 小 与 作用 力 成 正比 ,与 物体 的 质量 成 反比 ,可 以 
得 到 机 器 人 上 连 杆 质心 上 的 作用 力 下 与 相对 应 的 刚体 加 速度 的 关系 式 : 
F=m wv. (1.51) 
其 中 ,m 是 刚体 的 总 质量 。 
而 对 于 一 个 转动 的 刚体 ,还 要 分 析 引 起 刚体 转动 的 力矩 N。 欧 拉 方 程 用 来 表示 作用 在 
刚体 上 的 力矩 与 刚体 转动 的 角速度 和 角 加 速度 的 关系 : 
N —Io +w X lw (1.52) 
上 式 中 ,和 指 刚体 在 坐标 系 {C} 中 的 惯性 张 量 。 注 意 : 刚体 质心 的 位 置 位 于 坐标 系 原点 。 
有 了 上 面 两 个 方程 ,可 以 进一步 得 到 基于 机 械 臂 给 定 运动 轨迹 求解 驱动 力 或 力矩 的 方 
法 , 即 已 知 关节 的 位 姿 , 速 度 和 加 速度 分 别 为 9.4 和 和, 可 以 进一步 得 出 机 器 人 运动 的 驱 
动力 。 
这 种 计算 方法 可 分 为 两 步 。 
第 一 步 , 在 已 知 连 杆 位 置 g 的 情况 下 ,从 连 杆 1 到 连 杆 n 向 外 递 推 计算 连 杆 的 速度 g 和 
加 速度 4, 然 后 进一步 对 机 器 人 的 所 有 连 杆 使 用 牛顿 和 欧 拉 方 程 ,得 到 作用 在 连 杆 质心 上 的 
力 和 力矩 。 对 于 转动 关节 , 递 推 求解 的 具体 过 程 如 下 : 


Hom — UR; + Or Zia (1. 53) 
Hio "IR a: AIR IS; X fin 2m 4-021 Sn (1. 54) 
Him mIRC; XP m Hoi X Coi XP im Hv) (1. 55) 
He, SHR C on XPa Hwn X C oim XHP H Ou) (1. 56) 
P Fa = ma" e, (1.57) 
Na S [a gu, Hwn XC Das om (1.58) 


上 式 中 ,i 二 0,1,2,3,4,5。 对 于 通常 的 6 个 关节 均 为 转动 关节 的 机 器 人 ,通过 上 面 的 式 子 可 
以 求解 出 作用 在 每 个 连 杆 上 的 力 和 力矩 。 

第 二 步 ,计算 关节 力矩 。 实 际 上 ,动力 学 要 得 出 的 这 些 关节 力矩 是 施加 在 连 杆 上 的 力 和 
力矩 , 即 驱动 器 施加 在 机 器 人 上 的 力矩 或 作用 在 机 器 人 上 使 其 运动 的 外 力 。 而 这 种 求解 需 
要 使 用 向 内 递 推 的 方法 ,在 得 到 上 面 的 结果 后 ,具体 过 程 如 下 : 


‘fi —IRU fn HF: (1. 59) 
ini =Ni HARY na Pc XF: Pai XAR” fin (1. 60) 
"(— 2 a. 61) 


上 式 中 ,i 二 6,5,4,3,2,1。 上 面 的 式 子 即 为 通过 牛顿 - 欧 拉 递 推 法 推导 得 出 的 机 器 人 动力 学 
方程 。 
在 分 析 机 器 人 动力 学 的 过 程 中 .还 有 一 个 因素 不 能 忽视 . 那 就 是 重力 因素 ,各 连 杆 的 重 
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力也 要 加 入 到 动力 学 方程 中 。 由 于 递 推 推导 过 程 中 计算 力 的 时 候 会 使 用 到 连 杆 的 质量 和 加 
速度 ,所 以 可 以 假设 机 器 人 正 以 1g( 即 10m/s) 的 加 速度 向 上 做 加 速 运动 ,这 和 连 杆 上 的 重 
力作 用 是 等 效 的 。 因 此 可 以 让 线 加 速度 的 初始 值 与 重力 加 速度 大 小 相等 .方向 相反 ,这 样 ， 
不 需要 进行 其 他 附加 的 运算 就 可 以 将 重力 的 影响 加 入 到 动力 学 方程 中 。 

上 面 各 式 即 为 运用 牛顿 - 欧 拉 递 推 的 方法 通过 机 器 人 的 运动 轨迹 (即位 姿 、 速 度 和 加 速 
度 ) 得 到 机 器 人 的 期 望 驱动 力矩 的 具体 过 程 。 其 中 ,角速度 、 角 加 速度 和 线 加 速度 的 初始 值 


分 别 是 : 
"mU "D E 
"ev = |0 @ = |0 vo = 0 (1. 62) 
0 0 0 


4. 用 拉 格 朗 日 法 建立 机 器 人 动力 学 方程 

牛顿 - 欧 拉 法 是 通过 基于 由 牛顿 定律 和 欧 拉 方程 推导 出 作用 在 连 杆 上 的 力 和 力矩 从 而 
得 到 机 器 人 动力 学 的 方法 ,而 拉 格 朗 日 法 则 是 从 基于 能 量 的 角度 来 分 析 机 器 人 的 动力 学 。 
对 于 同一 个 机 器 人 ,两 者 得 到 的 动力 学 方程 是 相同 的 。 

首先 从 分 析 动 能 开始 : 对 于 机 器 人 的 第 i 个 连 杆 ,其 动能 可 以 表示 为 : 


ki = m; vg "uc, +E fo Ho, (1. 63) 


上 式 中 的 两 项 分 别 代表 由 连 杆 的 线 速度 (质心 处 ) 引 起 的 动能 和 由 连 杆 的 角速度 ( 同 为 质心 
处 ) 引 起 的 动能 。 则 整个 机 械 臂 的 动能 是 所 有 的 连 杆 的 动能 之 和 , 即 : 
e 5 d (1. 60D 
而 机 器 人 的 动能 又 可 以 和 之 前 的 惯性 矩阵 M(g) 建 立 等 式 , 对 于 6 关节 的 机 器 人 ,6 个 连 杆 
的 动能 可 以 由 6X6 矩阵 M(9) 与 关节 角速度 4 建立 关系 式 : 
klag) = TMD YG (1. 65) 


从 物理 力学 可 知 , 物 体 的 总 动能 总 是 为 正 ,所 以 惯性 矩阵 M(g) 为 正定 和 矩阵 。 
第 二 步 研究 机 器 人 的 势能 : 对 于 机 器 人 的 第 i 个 连 杆 ,其 势能 可 以 表示 为 : 
Ui =— mi *g* Pc, F tres, (1. 66) 
上 式 中 ,"g 是 3X1 的 重力 加 速度 矢量 ,"Pc, 是 第 i 个 连 杆 的 质心 的 相对 位 置 矢量; 取 常数 
ad 是 为 了 使 势能 最 小 为 0。 则 整个 机 械 辟 的 势能 是 所 有 的 连 杆 的 势能 之 和 , 即 : 


u 一 > us (1. 67) 
因为 "Pc 是 第 i 个 连 杆 质心 的 相对 位 置 矢量 , 则 "Pc 应 该 是 关节 角 的 函数 ; 机 械 臂 的 整体 势 
能 可 以 表述 为 u(1) ,是 各 关节 位 置 的 标量 函数 。 
当 得 到 机 器 人 的 动能 和 势能 后 ,进一步 推导 计算 得 到 拉 格 朗 日 函数 , 即 : 
L(q.d)— k(g.g)—u(g) (1. 68) 
通过 拉 格 朗 日 函数 得 到 机 器 人 的 驱动 力矩 : 
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rc (1. 69) 
9 
对 于 机 械 臂 ,驱动 力矩 方程 也 可 以 表示 为 ， 

d 29k 2k ,9u .. a. 70) 


digg q adg | 
这 样 通 过 机 器 人 的 动能 和 势能 , 即 拉 格 朗 日 函数 .就 能 得 到 机 器 人 的 驱动 力矩 ,这 就 是 
由 拉 格 朗 日 法 推导 机 器 人 动力 学 的 过 程 。 


1.4.3 ”状态 空间 方程 


1. 状态 空间 方程 

如 果 对 方程 进行 相关 的 归纳 和 分 类 ,然后 可 以 进一步 简化 ,并 且 很 简便 地 表示 机 械 臂 的 
动力 学 方程 。 其 中 有 一 种 就 是 用 状态 空间 方程 表示 动力 学 方程 。 不 考虑 一 切 摩擦 因素 ,其 
具体 形式 如 下 : 

Mai  C(q-q0d- GC FCD JéD* f — c (1.71) 

上 式 中 ,gE€ R" ,为 关节 角 位 移 量 ; MID ER” ,为 机 器 人 的 惯性 矩阵 ,该 矩阵 是 一 个 角 对 称 
矩阵 ,在 这 个 nox n 矩阵 中 ,里 面 的 非 零 元 素 的 大 小 取决 于 机 器 人 中 各 关节 角 aO 0s ns 
0,) 的 大 小 ; M(g)g 表 示 该 机 械 辟 受到 的 惯性 力 的 大 小 。C(g.9)E R" 为 科 里 奥 利 和 矩阵, 表 
示 离 心力 和 科 里 奥 利 力 ( 科 氏 力 ),V (g,4 )= 二 C(g,4)g 和 矩阵 中 非 零 元素 的 大 小 取决 于 两 个 
因素 一 一 机 器 人 中 各 关节 的 关节 角 g 及 其 关节 角速度 4 。G(d) ER" 表示 重力 矩阵 , 即 机 械 
臂 上 各 连 杆 的 重力 因素 , 它 表示 这 个 机 器 人 受到 重力 的 大 小 。G(g) 中 非 零 元 素 的 大 小 与 机 
器 人 各 关节 的 关节 角 g AK. FO HERDE JOTS 表示 关节 力 , 由 一 个 作用 在 末端 执 
行 器 的 扭力 f 产生 ,J 是 机 械 臂 的 雅 可 比 矩 阵 。rER" ,是 与 关节 角 位 移 量 g 有 关 的 广义 驱 
动力 向 量 。 

由 于 上 式 中 的 离心 力 和 科 氏 力矩 阵 C(q,q ) 取 决 于 机 械 臂 各 关节 连 杆 的 位 置 和 速度 ,所 
以 将 这 个 方程 式 称 为 状态 空间 方程 。 

2. 各 项 参数 的 获取 与 分 析 

MATLAB 机 器 人 工具 箱 提供 了 一 些 函 数 ,可 以 提取 工具 箱 中 已 定义 的 机 器 人 模型 的 
动力 参数 。 下 面 以 puma560 机 器 人 为 例 . 对 函数 进行 说 明 ( 机 器 人 工具 箱 , 定 义 了 许多 机 器 
人 模型 ,其 中 只 有 puma560 机 器 人 的 定义 中 涉及 动力 参数 ) 。 

1) 运动 学 和 动力 学 参数 

可 以 用 函数 SerialLink. dyn() 来 显示 机 器 人 某 个 连 杆 的 运动 学 参数 和 动力 学 参数 。 例 
如 ,显示 puma560 机 器 人 第 6 个 连 杆 的 参数 。 

输入 命令 : 


>> p560. links(6).dyn 
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运行 结果 为 : 
theta-q, d= 0,a- 0, alpha= O0, offset- 0 (R, stdDH) 
了 0.09 
4m 0 0 0.032 
I =| 0.00015 0 0| 
| 0 0.00015 0| 
| 0 0 4e-05 | 
Jm = — 3.3e-05 
Bm = — 3.67e- 05 
Tc = 0.00396(*)  -0.0105(-) 
6 = 76.69 
qlim = -4.642576 to 4.642576 


可 以 得 到 以 下 信息 : 第 6 个 连 杆 的 运动 学 DH 参数 , 连 杆 质量 ,质心 的 坐标 ,惯性 矩阵 ， 
电机 转动 惯量 ,电机 摩擦 力 ,库仑 力 和 齿轮 传动 比 。 

2) 惯性 矩阵 

当 机 器 人 的 关节 角 为 4 时 ,可 以 通过 函数 SerialLink. inertia() 获 取 机 器 人 的 惯性 矩阵 。 
例如 , 当 gq 三 [0 00000] 时 , 求 出 相应 的 惯性 矩阵 。 

输入 命令 : 


>>q=[0000 00]; 
>> p560. inertia(q) 


运行 结果 为 
ans = 
3.9611 一 0.1627  - 0.1389 0.0016 -0.0004 0.0000 
— 0.1627 4.4566 0.3727 0.0000 0.0019 0.0000 
—0.1389 0.3727 0.9387 0.0000 0.0019 0.0000 
0.0016 0.0000 0.0000 0.1924 0.0000 0.0000 
-0.0004 0.0019 0.0019 0.0000 0.1713 0.0000 
0. 0000 0.0000 0.0000 0.0000 0.0000 0.1941 
得 到 惯性 矩阵 M 为 : 
3.9611 一 0.1627 一 0.1389 0.0016 一 0.0004 0 
一 0.1627 4. 4566 0.3727 0 0.0019 0 
— 0. 1389 0. 3727 0.9387 0 0.0019 0 
Mq) = 
0. 0016 0 0 0. 1924 0 0 
— 0. 0004 0. 0019 0. 0019 0.1713 0 
0 0 0 0 0. 1941 


能 够 看 出 ,惯性 矩阵 MD Jy — A fa XER HERE o 
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3) 科 里 奥 利 矩 阵 
当 机 器 人 的 关节 角 为 g 时 ,关节 角 的 速度 为 4 ,可 以 通过 函数 SerialLink. coriolis() 获 取 
机 器 人 的 科 里 奥 利 矩 阵 。 


例如 , 当 9=[0 0 0 0 0 0], 每 个 关节 的 角速度 为 30s,a=| B. Bi pi pi 
BOR ,来 出 相应 的 惯性 年 隆 ， 
输入 命令 : 
>>q=[00000 0]; 
>>qd= [pi/6 pi/6 pi/6 pi/6 pi/6 pi/6]; 
>> p560. coriolis(q, qd) 
运行 结果 
ans = 
-0.4206 -0.5773 -0.2121 - 0.0007 -0.0014 0.0000 
0.2118 -0.2029 - 0.4050 -0.0000 - 0.0020 0 
0.2081 — 0.2021 -0.0000 0.0000 - 0.0001 0 
0.0000 — 0.0000 0.0000 0 0 0 
0.0007 — 0.0007 0.0001 0 0 0 
0 0 0 0 0 0 
得 到 科 氏 矩阵 C(q,9 ) 为 : 
0.4206 一 0.5773 一 0.2121 0.0007 一 0.0014 0 
0.2118 — 0.2029 — 0.4050 0 —0.0020 0 
: 0. 2081 0. 2021 0 0 —0.0001 0 
C(q.q)— 
0 0 0 0 0 0 
0.0007 0.0007 0.0001 0 0 0 
0 0 0 0 0 0 
D 重力 矩阵 


当 机 器 人 的 关节 角 为 g 时 ,可 以 通过 函数 SerialLink. gravload() 获 取 机 器 人 的 重力 
矩阵。 

例如 , 当 gq 二 [0 0 0 0 0 0], 得 到 相应 的 重力 矩阵 。 

输入 命令 : 


?»g-[000000]; 
>> p560. gravload(q) 


运行 结果 为 : 
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ans 
0 37.4837 0.2489 0 0 0 


得 到 重力 矩阵 为 : 


G(q)=(0 37.4837 0.2489 0 0 0)* 


5) 摩擦 力矩 

工具 箱 中 没有 对 摩擦 力矩 进行 直接 计算 的 函数 ,从 函数 SerialLink. dyn() 可 以 得 到 跟 
摩擦 力矩 相关 的 黏 性 摩擦 系数 .库伦 摩 氛 系 数 和 传动 比 的 参数 。 例 如 前 面 的 p560. links(6) 
.dyn 函数 的 返回 值 中 : 

。Bm= 3.67e—05 为 黏 性 摩擦 系数 ; 

e Tc = 0.00396( 十 ) 一 0.0105( 一 ) 为 库仑 摩擦 系数 ; 

* G—76.69 为 齿轮 传动 比 。 

3. 逆向 动力 学 的 计算 

MATLAB 机 器 人 工具 箱 中 使 用 函数 SerialLink. rne() 计 算 动力 学 的 逆 问 题 ,其 中 主要 
该 函数 的 参数 主要 为 g( 关 节 角 ) ,qd (速度 ) ,gdd( 加 速度 ) ,grav( 重 力 项 ,默认 下 为 地 球 的 重 
力 项 ) 。 

以 puma560 机 器 人 为 例 , 设 置 关节 角 为 gq 二 (0 0 0 30° 30° 30?), 关 节 角 速度 4 
为 (0 0 0 10° 10° 10"), 加 速度 为 (0 0 0 0 0 0, 

输入 语句 : 


>> mdl puma560; 

»» q= [0 0 0 pi/6 pi/6 pi/6]; 
>>qd= [0 0 0 pi/18 pi/18 pi/18]; 
>>qdd= [00000 0]; 

>> t1 = p560. rne(q, ad, add) 


-0.0000 37.4713 0.2366 0.9235 0.7265 0.3413 
可 以 得 到 ,此 时 的 驱动 力矩 为 : 
rl= (0 37.4713 0.2366 0.9235 0.7265 0.3413) 

当 忽略 掉 状 态 方程 的 重力 项 时 ,输入 语句 : 


>> t2 = p560. rne(q, ad, add, [0 0 0]") 
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一 0.0000 -0.0001 .- 0.0001 0.9235 0.7406 0.3413 


可 以 得 到 ,此 时 的 驱动 力矩 为 : 


r2=(0 -0.0001 -0.0001 0.9235 0.7406 0.3413) 


此 外 ,函数 SerialLink. rne() 也 可 以 计算 机 器 人 沿 着 一 条 轨迹 运动 时 ,每 一 个 时 刻下 的 
驱动 力矩 。 


输入 语句 : 

>> T1 =trans1(0.3,0.1,0) * trotx(pi); 名 设置 初始 位 次 

>> ql = p560. ikine6s(T1); 多 计算 对 应 关节 角 

>> T2- trans1(0.2,0.4,0) * trotx(pi)/2; ”名 设置 最 终 位 姿 

>> q2 = p560. ikine6s(T2) ; 多 计算 对 应 关节 角 

> t- [0:0.1:6]'; 名 设 置 时 间 及 步 长 

>> [qqd,qdd] = jtraj(q1,q2, t); 名 生成 相应 的 轨迹 

>> tu= p560. rne(q, ad, add) ; 外 计算 轨迹 上 每 个 点 的 驱动 力矩 
运行 结果 : 

0.0000 -15.5857 -2.6777 -0.0000 0.0000 0 


24.7760 — 23.2628 4.4214 - 1.2850 -1.0435 0.3066 
24.6978 — — 17.2257 5.5386 -1.2850 -1.0424 0.3010 
0.0000 —9.5899 -1.5434 -0.0000 -0.0000 0.3037 


结果 得 到 了 一 个 61X6 的 矩阵 ,每 一 行 都 对 应 着 某 一 个 时 间 点 的 驱动 力矩 。 更 直观 地 
观察 每 个 关节 的 驱动 力矩 随时 间 的 变换 ,可 以 用 图 形 表 示 , 输 入 语句 : 


7» plot(t,tu(:,1)); 
>> hold on 

>> plot(t,tu(:,2)); 
>> plot(t,tu(:,3)); 
2» plot(t,tu(:,4)); 
>> plot(t,tu(:,5)); 
2» plot(t,tu(:,6)); 


运行 结果 如 图 1-28 所 示 。 
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图 1-28 机 器 人 的 各 个 关节 在 力矩 的 驱动 下 运行 的 轨迹 


1.4.4 正 向 动力 学 
1. 正 向 动力 学 的 计算 方法 
通过 状态 空间 方程 进行 推算 ,可 以 得 到 动力 学 方程 中 的 加 速度 : 
q — M(g) (r—C(g.g9)g—G(g)+ F(q)) (2. 72) 
可 以 通过 简单 的 欧 拉 积 分 方法 ,计算 出 机 械 臂 的 位 置 . 速 度 和 加 速度 。 设 q (000 — qo 
d (00 —0. (—0 时 开始 ,进行 迭代 计算 。 


当时 刻 为 :十 At, 机 械 辟 的 速度 为 : 
qGO AD — GHOD At (1. 73) 
此 时 ,机械 臂 的 位 置 为 ， 
qG - AD — DHH d IOS . 74) 


然后 将 gq 三 g GT AD 和 g =å (十 At) ,代入 到 上 面 的 方程 能 够 得 到 机 械 臂 的 加 速度 
q GF AD « 。 对 于 一 个 机 械 臂 来 说 ,输入 一 定 的 驱动 力矩 r 时 ,就 可 以 通过 上 面 的 方法 ,得 到 
每 一 个 时 刻 的 机 械 臂 位 置 .速度 和 加 速度 。 此 外 ,关于 数值 积分 的 方法 ,实现 欧 拉 积分 ,还 有 
其 他 的 方法 ,这 里 不 做 详 述 。 

2. MATLAB 计算 正 向 动力 学 

MATLAB 机 器 人 工具 箱 中 提供 了 函数 SerialLink. fdyn() 计 算 正 向 动力 学 ,主要 的 调 
用 格式 为 : [T.q.qd] —SerialLink. fdynC T, torqfun)。 其 中 , 工 表示 时 间 间 隔 (采样 时 间 )， 
torqfun 表示 给 定 的 力矩 函数 ,根据 力矩 函数 可 以 求 出 相对 应 的 关节 角度 和 关节 角速度 。 

此 外 ,MATLAB 机 器 人 工具 箱 中 提供 了 函数 SerialLink. accel O ,可 以 计算 给 定 关节 
ff 关节 角速度 .关节 角 驱 动力 矩 时 ,相对 应 的 关节 角 加 速度 。 

给 定 关节 角 : g 二 (0 0 0 0 0 05; 

关节 角速度 : 4 二 (0 0 0 0 0 0); 
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关节 角 驱 动力 矩 : = 1 1 1 1 D. 
计算 关节 角 加 速度 ,输入 命令 : 


>> mdl puma560 
>>q=[0000 0 0]; 
>>qd=[00 00 0 0]; 
2» t-2[111111]; 
>> qdd = p560. accel(aq, ad, t) 


3. Simulink 计算 正 向 动力 学 

MATLAB 机 器 人 工具 箱 中 提供 了 一 些 用 于 机 器 人 仿真 的 Simulink 文件 ,其 中 包含 着 
一 个 这 样 的 例子 : puma560 在 零 关 节 力 矩 的 情况 下 ,受到 重力 作用 导致 机 械 臂 在 仿真 时 就 
立即 下 附 的 结果 。 这 里 使 用 该 例子 ,并 改变 初始 的 力矩 ,阐明 如 何 用 Simulink 计算 正 向 动 
力学 。 

输入 语句 : 


>> sl_ztorque 


通过 以 上 的 语句 , 即 可 加 载 已 创建 的 Simulink 文件 ,对 该 模型 进行 修改 ,如 图 1-29 
所 示 。 


6 
Puma 560 q 


图 1-29 MATLAB 工具 箱 中 Simulink 模型 (一 ) 
输入 力矩 为 1 二 [20 20 20 20 20 20]. 并 进行 仿真 .输入 语句 : 


>> r= sin('sl ztorque') 
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可 以 看 到 机 器 人 在 驱动 力矩 的 作用 下 进行 运动 ,并 输出 机 器 人 运动 过 程 中 变化 的 时 间 
和 关节 和 角 ( 这 里 的 输入 力 抢 可 能 与 具体 的 情况 不 符 ,在 实际 的 仿真 工作 中 ,应 当 设 置 合理 的 
力矩 值 ): 


Simulink.SimulationOutput: 
tout: [161x1 double] 
yout: [161x6 double] 


可 以 获取 时 间 与 关节 角 ,并 绘制 出 关节 角 随 时 间 变 化 的 图 形 : 
输入 语句 : 

>> t-r.find('tout'); 

>> q= r.find('yout'); 

7» plot(t,q(:,1:6)) 


1.4.5 机 器 人 工具 箱 的 SerialLink 类 2 


根据 Robotics Toolbox for MATLAB 一 书 , 下 面 对 SerialLink 类 涉及 机 器 人 动力 学 内 
容 进 行进 一 步 的 解析 。 

1. SerialLink. cinertia: 笛 卡 儿 惯 性 矩阵 

m = R, cinertia(q) 笛 卡 儿 ( 操 作 空 间 ) 惯 性 矩阵 ,其 将 笛 卡 儿 力 /扭矩 与 关节 配置 g 处 
的 笛 卡 儿 加 速度 相关 。 

2. SerialLink. collisions; 执行 碰撞 检查 

COD 如 果 姿 态 gq(1XN) 下 的 SerialLink 对 象 R 与 属于 CollisionModel 类 的 实体 模型 相 
交 , 则 C = R.collisions(q,model) 为 真 。 该 模型 包括 多 个 几何 基 元 和 相关 姿势 。 

(2) C = R.collisions(q. model.dynmodel.tdyn) 如 上 所 述 , 但 也 检查 其 元 素 处 于 姿 
35 tdyn 的 动态 碰撞 模型 dynmodel。tdyn 是 一 个 变换 矩阵 (4X4XP) 的 数组 ,其 中 忆 为 长 
度 (dynmodel_primitives) 。tdyn 的 第 P 个 平面 预 取 dynmodel 的 第 已 个 原 语 的 姿态 。 

(3) C = R. collisions(q,model,dynmodel) 如 上 所 述 , 但 假设 tdyn 是 机 器 人 的 工具 
HER, WMR qg 是 MXN 为 姿态 序列 ,C 为 MX1, 并 且 冲 突 值 应 用 9 矩阵 相应 行 的 姿态 。 
tdyn 是 4X4XMXP。 

3. SerialLink. dyn: 显示 惯性 属性 

(1) R. dyn() 以 多 行 格式 显示 SerialLink 对 象 的 惯性 属性 。 所 示 的 属性 是 质量 , 质 
心 , 惯 性 ,齿轮 比 , 电 机 惯量 和 电机 摩擦 。 

(2) R. dyn(J) 如 上 所 述 , 但 仅 显 示 关 节 J 的 参数 。 

4. SerialLink. fdyn: 用 于 计算 机 器 人 的 正 向 动力 学 

(OD [T.q.qd] = R.fdyn(T,torqfun) 在 0 到 了 的 时 间 间 隔 上 对 机 器 人 的 动力 学 进 
行 积分 ,并 返回 时 间 工 ,关节 位 置 g 和 关节 速度 gd 的 向 量 。 初 始 关节 位 置 和 速度 为 零 。 施 
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加 到 关节 的 扭矩 由 用 户 提供 的 控制 函数 torqfun 计算 : 


TAU = TORQFUN(T, Q, QD) 


其 中 ,Q 和 QD 分 别 是 操纵 器 关节 坐标 和 速度 状态 , 工 是 当前 时 间 。 

(2) [ti.q.qd] = R.fdyn(CT,torqfun,q0,qd0) 允许 指定 初始 关节 位 置 和 速度 。 

(3) [T.q.qd] = R. fdyn(Tl,torqfun,q0,qd0,ARG1,ARG2,…) 允许 将 可 选 参数 
传递 给 用 户 提 供 的 控制 函数 : 


TAU = TORQFUN(T,Q,QD, ARG1, ARG2, …) 


例如 ,如 果 机 器 人 由 PD 控制 器 控制 ,可 以 定义 一 个 函数 来 计算 以 下 控制 函数 


tau = mytorgfun(t, q, ad, qstar, P, D) 
tau = P *(qstar- q) * D * qd; 


然后 将 机 器 人 动力 学 与 控制 器 集成 


[t,q] = robot. fdyn(10, @mytorqfun, qstar, P, D) 


iE. 

(1) 此 函数 对 非 线性 关节 摩擦 (例如 库仑 摩擦 ) 执 行 效 果 较 差 。R. nofriction 方法 可 用 
于 将 此 摩擦 设置 为 零 。 

(2) 如 果 未 指定 torqfun, 或 给 定 为 0 或 卜 , 则 无 捏 矩 施加 到 机 械 手 关节 。 

(3) 使 用 内 置 积 分 函数 ode45() 。 

5. SerialLink. friction: 机 器 人 的 摩擦 力 

它 是 机 器 人 以 关节 速度 qd 移动 的 关节 摩擦 力 /扭矩 的 矢量 。 

摩擦 模型 包括 : 

。 速度 线性 函数 的 黏 性 摩擦 力 。 

。 与 qd 成 比例 的 库仑 摩擦 。 

6. SerialLink.gencoords: 符号 广义 坐标 向 量 

q = R. gencoords() 是 符号 Lql q2 … qN] 的 向 量 (1XN) 。 

[q,qd] = R. gencoords() ,qd 是 符号 的 向 量 (1 XN)[qdl qd2 … qdN]. 

[q,qd,qdd] = R. gencoords() ,qdd 是 符号 Lqddl ,qdd2,… . qddN ] ff [8] fit C10 ND 。 

7. SerialLink.genforces: 矢量 符号 广义 力量 

q = R. genforces() 是 符号 LQ1 Q2 … QN] 的 向 量 (1 XN)。 

8. SerialLink.getpos: 从 图 形 显示 获取 关节 坐标 

q = R. getpos() 返 回 图 形 机 器 人 上 的 最 后 一 个 绘图 或 给 出 操作 设置 的 关节 坐标 。 
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9. SerialLink. gravjac() 

D 快速 重力 荷载 和 和 雅 可 比 

[tau.jac0] = R.gravjac(q) 由 机 器 人 处 在 于 姿态 q(l1XN) 时 ,由 于 重力 (1XN) 和 机 
械 璧 的 雅 可 比 产生 的 广义 关节 力 / 扭 矩 ,其 中 NN 是 机 器 人 关节 的 数量 。 

[ga,jac0] = R.gravjac(q.grav) 与 上 面 的 相似 ,但 重力 由 grav(3X1) 明 确 给 出 。 

2) 轨迹 操作 

如 果 4 是 MXN, 其 中 NN 是 机 器 人 关节 的 数量 , 则 4 的 每 一 行 对 应 于 假定 轨迹 的 姿态 。 
tauC M X N) 是 广义 的 关节 力矩 ,每 行 对 应 于 输入 姿态 ,jac0(6X N XM) ,其 中 每 个 平面 是 对 
应 于 输入 姿态 的 雅 可 比 行列 式 。 

iE. 

(1) 如 果 没 有 明确 给 出 ,重力 失 量 由 SerialLink 属性 定义 。 

(2) 不 使 用 逆 动 力学 函数 RNE。 

(3) 比分 别 计算 重力 和 雅 可 比 更 快 。 

10. SerialLink. gravload: 关节 重力 负荷 

(D taug = R. gravload(q) ”对 关节 配置 q(1 XN) 中 机 器 人 R 的 关节 重力 加 载 (LX N)， 
其 中 NN 是 机 器 人 关节 的 数量 。 重 力 加 速度 是 机 器 人 对 象 的 属性 。 

如 果 q 是 矩阵 (MX ND , 则 每 行 被 解释 为 联合 配置 向 量 , 并 且 结 果 是 : 每 行 是 对 应 的 关 
节 扭 矩 的 矩阵 (MX ND. 

(2) taug = R. gravload(q,grav) 如 上 ,但 重力 加 速度 向 量 grav 被 明确 给 出 。 


1.5 机 器 人 的 运动 轨迹 


1.5.1 运动 轨迹 问题 

机 械 臂 在 三 维 空间 中 每 个 关节 的 位 置 .速度 和 加 速度 都 是 关于 时 间 的 函数 ,它们 构成 了 
机 械 臂 的 运动 轨迹 。 关 于 机 械 臂 的 运动 轨迹 主要 有 3 个 问题 : 根据 具体 的 操作 任务 给 机 械 
臂 指定 一 条 空间 中 的 轨迹 ; 描述 一 条 规划 好 的 轨迹 ; 与 轨迹 生成 相关 的 问题 。 

关于 机 械 臂 的 位 姿 描 述 的 方法 ,一 共有 3 种: 关节 空间 描述 、 驱 动 器 空间 描述 和 笛 卡 儿 
空间 描述 。 确 定 一 个 nn 自由 度 机 械 辟 的 所 有 连 杆 位 管 ,需要 一 组 个 关节 变量 的 关节 矢量 ， 
所 有 的 关节 矢量 组 成 了 关节 空间 。 将 关节 矢量 表示 成 一 组 驱动 器 函数 , 称 为 驱动 器 矢量 。 
所 有 的 驱动 器 矢量 组 成 了 驱动 器 空间 。 当 机 械 辟 的 位 管 是 在 空间 相互 正 交 的 轴 上 测量 、 姿 
态 按 照 欧 拉 角 等 规定 测量 时 , 称 这 个 空间 为 笛 卡 儿 空 间 。 


1.5.2. 关节 空间 的 规划 方法 


使 用 五 阶 多 项 式 作为 路 径 段 ,确定 路 径 段 的 起 始点 和 终止 点 的 位 置 . 速 度 和 加 速度 , 需 
要 用 一 个 五 次 多 项 式 进 行 插值 。 
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O) — ao Hait Hazt? 十 ast? + aut dr as (1.75) 
ARREK: 
bo = ao a. 76) 
br = ao ats azt} +azth + aat] + ast? (77) 
Ó-—a d. 78) 
Ò; = ai + 2ast, + 3ast5 + Aa 413 + Basti (2. 79) 
o = 2a; (1. 80) 
07 = 2a; + 6asty + 12a4t5 + 202515 (2.81) 
可 以 由 上 面 的 方程 组 得 到 相应 的 解 : 
ay = bo (1. 82) 
ay = ĝo a. 83) 
ai £ (1. 84) 
200; — 200 — (8 0j +20 Go) — (GO, — 00d ER 
2t} 
a, — 300 — 300, — C14 r+ 16 Godts + (G0s — 20015 (1.86) 
2th 
a, = 120—120 — (60, - 601 — (0. — 0015 aus) 


2 好 
MATLAB 机 器 人 工具 箱 中 提供 的 函数 tpoly() 可 以 生成 五 次 多 项 式 轨迹 。 例 如 ,生成 
一 个 初始 位 置 为 0, 最 终 位 姿 为 4, 初 速度 为 2, 最终 速度 为 0, 最初 加 速度 和 最 终 加 速度 都 为 
0, 时 间 长 度 为 20 的 轨迹 时 : 
输入 命令 : 


>> [x v a] = tpoly(0,4,20,2,0) ; 
2» plot(x) 


运行 结果 如 图 1-30 所 示 。 

1. 关节 角 空 间 的 轨迹 

以 KUKA KR5 机 器 人 为 例 ,利用 MATLAB 机 器 人 工具 箱 , 对 机 器 人 末端 执行 器 在 关 
节 角 空间 中 两 个 位 姿 之 间 移 动 的 轨迹 进行 仿真 。 

初始 位 姿 ; 


T1 = trans1(0.3,0.1,0) x trotx(pi); 
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> 
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图 1-30 tpoly() 生 成 的 五 次 多 项 式 轨迹 


此 时 ,对 应 的 一 组 关节 角 为 : 


TE DERE 


此 时 ,对 应 的 一 组 关节 角 为 : 


设置 机 器 人 末端 执行 器 从 A 点 移动 到 B 点 ,用 时 6s, 每 100ms 计算 一 次 关节 角 : 


求解 A 点 到 B 点 ,6 个 关节 角 在 6s 中 的 变换 过 程 ,MATLAB 机 器 人 工具 箱 中 可 以 用 
以 下 三 条 等 价 的 语句 进行 计算 : 


结果 运行 如 下 : 


第 1 章 ” 机 器 人 学 与 MATLAB 机 器 人 工具 箱 |» 69 


生成 了 一 个 61X6 的 矩阵 ,可 以 看 出 从 A 点 到 B 点 的 移动 过 程 中 ,关节 角 由 91 到 g2 
逐渐 逼近 。 此 外 ,还 可 以 通过 可 选 参数 求 出 从 A 点 到 召 点 移动 过 程 中 的 速度 和 加 速度 。 

语句 : [q v,a] = jtraj(ql,q2,t); 同 样 为 速度 v、 加 速度 a 各 自生 成 了 一 个 6156 的 矩阵 。 

下 面 将 从 动画 、 图 形 对 机 械 臂 的 轨迹 进行 显示 : 

当 用 动画 对 该 过 程 进行 仿真 时 ,相关 的 语句 为 : 


KR5. plot(q); 
需要 绘制 所 有 关节 角 随 时 间 变 化 的 图 形 时 ,相关 的 语句 为 : 
>> qplot(t, q) 


运行 结果 如 图 1-31 所 示 o 
1 需要 绘制 第 m 个 关节 角 随 时 间 变 化 的 图 形 时 ,相关 语句 为 : plot(t,q(:,n)), 例 如 : 


运 
当 只 


>>plot(t,q(:,1)) 


运行 结果 如 图 1-32 所 示 。 


图 1-31 所 有 关节 角 随时 间 变化 图 图 1-32 关节 角 ql 随时 间 变 化 图 
使 用 下 列 代码 可 以 求 出 末端 执行 器 在 笛 卡 儿 空间 中 的 移动 轨迹 。 


>> T3 = KR5. fkine(q); 
>> p= transl(T3); 
7? plot(p(:,1),p(:,2)) 


运行 结果 如 图 1-33 所 示 。 
2. 第 卡 儿 空 间 的 轨迹 
绘制 末端 执行 器 在 cy 平面 上 的 运动 轨迹 ,输入 命令 : 
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>> Ts = ctraj(T1, T2, length(t)) ; 
>> plot(t,transl(Ts)) 


运行 结果 如 图 1-34 所 示 。 


0.22 0.3, 
92 0.25 
0.18 
02 
0.16 
ois} 
0.14 
0.12 9t 
0.1 0.05, 
| SR . 
0.10.12 0.14 0.16 0.18 0.2 0220.24 0.26 0.28 0.3 D— cL. — 79. 2X7 ME T USCTCUB 
图 1-33. 未 端 执行 器 在 笛 卡 儿 空间 中 的 移动 轨迹 图 1-34 末端 执行 器 在 zy 平面 的 轨迹 


绘制 末端 执行 器 在 zy 平面 的 指向 轨迹 ,输入 命令 : 


>> plot(t, tr2rpy(Ts)) 


运行 结果 如 图 1-35 所 示 。 
4 


eo — N € 


0 1 2 3 4 5 6 
图 1-35 末端 执行 器 在 zy 平面 的 指向 轨迹 


1.6 机 械 臂 关节 控制 


1.6.1 机 器 人 控制 系统 的 构成 


在 实际 中 ,机 器 人 控制 系统 的 物理 构成 如 图 1-36 所 示 。 通 过 示 教 .数值 数据 或 外 传 感 
器 来 生成 目标 轨迹 ,转换 成 笛 卡 儿 坐标 系 下 的 机 器 人 末端 执行 器 的 坐标 轨迹 ; 因 机 械 臂 各 
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关节 的 运动 轨迹 是 在 关节 空间 里 ,所 以 需要 通过 目标 轨迹 生成 环节 将 笛 卡 儿 空 间 的 坐标 轨 
迹 转换 成 机 器 人 各 个 机 械 辟 的 转角 轨迹 , 即 为 机 器 人 各 关节 的 期 望 输出 ; 控制 器 通过 相关 
的 控制 算法 处 理 并 输出 控制 量 , 执 行 器 根据 控制 量 的 大 小 输出 力矩 驱动 机 器 人 各 关节 ,最 终 
使 系统 稳定 并 保证 跟踪 误差 ( 即 : 期 望 值 与 实际 值 之 差 ) 收 剑 到 零 或 零 附 近 的 一 个 区 域 , 同 
时 满足 一 定 的 动态 性 能 指标 。 


JU pesha | 控制 量 
数值 数据 e 指令 信号 gE = em -| 执行 器 E 


外 传感器 


图 1-36 机 器 人 控制 系统 物理 构成 示意 图 
用 X 表示 机 器 人 末端 执行 器 在 笛 卡 儿 坐标 系 下 的 期 望 运行 轨迹 ,可 根据 逆 运 动 学 由 和 
求 取 出 机 器 人 各 关节 的 期 望 转角 qa ,其 转角 速度 及 其 加 速度 分 别 用 gs da 表示 ,用 jy 表示 控 
制 器 的 输入 , 则 机 器 人 控制 系统 的 框图 形式 如 图 1-37 所 示 。 


X us da» da - a "m 
轨迹 生成 n ^ $^ 控制 器 [LI e 执行 器 | ^ MGpi*Cq. dd* Gaye 


人 外 kh 


' 


图 1-37 机 器 人 控制 系统 框图 


1.6.2 Simulink 机 器 人 模块 


机 器 人 工具 箱 中 提供 了 一 些 机 器 人 模块 ,存放 的 目录 路 径 为 toolbox \ rvetools \ 
simulink, 可 以 通过 以 下 的 命令 显示 工具 箱 中 所 有 的 机 器 人 模块 .输入 命令 : 


>> roblocks 


运行 结果 如 图 1-38 所 示 。 

可 以 看 出 , 它 包括 了 图 形 模 块 、 轨 迹 模块 、 变 换 转 换 模 
块 .运动 学 模块 、 变 换 模 块 动 力学 模块 。 用 鼠标 双击 可 进 
入 下 一 级 的 子 目 录 , 如 图 1-39 所 示 , 动 力学 模块 提供 的 
Simulink 模块 ,包括 了 关节 机 器 人 模块 、 移 动机 器 人 模 
块 .飞行 器 模块 和 关节 控制 模型 。 

此 外 ,机 器 人 工具 箱 针对 机 器 人 控制 的 方法 ,提供 了 


一 些 Simulink 仿真 模型 ,下 面 将 以 这 些 Simulink 仿真 模 
1-38 工具 箱 中 的 机 器 人 模块 库 
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d Robotics Toolbox for MATLAB 


Amtype robot 


BENEA 


RNE 


Mobile robot 
移动 机 器 人 


Quadmtor 


飞行 器 


Joint control 


关节 控制 


图 1-39 
型 为 例 ,介绍 一 些 机 器 人 控制 的 方法 。 
1.6.3 机 器 人 的 单 关节 控制 
1. 电机 的 动力 模型 


工业 机 器 人 一 般 使 用 直流 (DC) 力 矩 电机 作为 驱动 器 。 


设 电 机 驱动 器 提供 的 电流 为 : 
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i = Kau (1. 88) 
它 产 生 力 矩 的 能 力 用 电机 转角 常数 开 。 来 描述 ,因此 , 电 枢 电 流 与 电机 产生 的 转 矩 的 关 
系 为 : 
t= Kni (1. 89) 
电机 动力 学 模型 可 表示 为 : 
Jo + Bo +t: (9) — Kn Kau (1. 90) 
其 中 ,J 是 电机 总 的 惯性 ,B XE B ERE BRI r EEO EE TRIB. 
2. 单 关节 控制 的 思想 
机 器 人 单 关节 控制 的 常见 方法 是 将 每 个 关节 当 作 一 个 独立 的 控制 系统 ,让 它 去 跟随 各 
自 的 关节 轨迹 。 因 此 ,机 器 人 每 个 关节 可 以 看 作 一 个 独立 的 电机 模型 ,因此 可 以 借鉴 电机 的 
双环 控制 ,其 中 内 层 为 速度 层 。 
3. 速度 环 
在 机 器 人 工具 箱 中 给 出 了 肩 关节 的 速度 控制 环 Simulink 模型 。 
忽略 电机 的 库仑 摩擦 力 , 对 电机 动力 模型 进行 拉 普 拉 斯 变换 得 到 : 
sJQ Go4- BONA(s)= Km KU (s) (1.91) 
HP ROERE S o GO f pr SELBE LU GO di xi 的 拉 普 拉 斯 变换 ,可 以 进一步 推算 
得 到 电机 的 传递 函数 : 


(+B)QC)= Kn K.U (s) 1.92) 
Qs) _ KaK, 
UG) B Sis 


1) 比例 控制 
基于 期 望 速度 和 实际 速度 之 间 的 误差 的 比例 控制 器 控制 律 为 : 
u' =K (ġà) a. 94) 
比例 控制 的 框图 如 图 1-40 所 示 ,其 中 ,电机 动力 学 模型 的 传递 函数 如 上 式 所 述 ; 控制 
周期 为 Ims, 它 用 于 仿真 控制 算法 的 计算 时 间 ; 力矩 限制 器 用 于 限制 电机 的 输入 力矩 ,防止 
输入 的 力矩 过 高 ,不 符合 实际 工作 情况 ; Ko 为 比例 控制 的 增益 。 


图 1-40 比例 控制 框图 


74 4| 机 器 人 仿真 与 编程 技术 
机 器 人 工具 箱 提供 了 一 个 Simulink 对 puma560 肩 关 节 进 行 比 例 控 制 的 模型 ,运行 下 
列 命令 : 
>> vloop test 


加 载 得 到 的 Simulink 模型 如 图 1-41 所 示 。 


信号 发 生 器 
qderror 
e 
wed 具有 链接 懂 性 的 puma560 
vases 具有 的 
扰动 转 矩 答 入 肩 关节 vloop 参 数 i 
积分 


图 1-41 Simulink vloop_test 模型 


相 比 于 图 1-40 所 示 的 比例 控制 器 ,本 模型 包含 了 一 个 输入 扰动 : 重力 力矩 项 , 它 的 值 
设置 为 20N。m。 这 里 的 vloop 模型 为 图 1-41 的 模型 ,但 多 了 一 个 积分 环节 ,因此 双击 该 模 
块 ,将 K, 设置 为 1,K; 设置 为 0, 使 它 为 增益 为 1 的 纯 比 例 控制 器 。 此 外 ,将 信号 发 生 器 设 
置 为 sawtooth, 即 为 锯齿 波 测试 信号 。 

运行 仿真 结果 如 图 1-42 所 示 。 

2) 比例 积分 控制 

比例 积分 控制 器 控制 律 为 : 


u* = (K. +É) - b. K:>0 a. 95) 


比例 控制 的 框图 如 图 1-43 所 示 。 
同样 地 .利用 上 面 的 Simulink 模型 进行 测试 ,运行 下 列 命令 : 

2» vloop test 

在 这 里 ,将 K, 设置 为 1, 将 天 ;设置 为 10, 因 此 ,该 控制 器 为 一 个 比例 增益 为 1, 积 分 增 


益 为 10 的 比例 -积分 控制 器 。 
运行 仿真 的 结果 如 图 1-44 所 示 。 
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g * * z + * * 加 


(a) 期 时 关节 角 和 实际 关节 角 


T T T T T T 


(b) 期 里 关节 角 和 实际 关节 角 误 差 


T T T T T T T T T 


c) 反馈 得 到 的 电机 力矩 


142 运行 仿真 结果 1 


gderror 
CD—e-( Oz 
d 控制 时 间 


图 1-43 ”比例 -积分 控制 框图 
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(a) 期 望 关节 角 和 实际 关节 角 


(b) MRK HAREK T fa iE 


' T T T T T T T T T 


“人 反馈 得 到 的 电机 力矩 
图 1-44 运行 仿真 结果 2 
3) 前 馈 控 制 
前 馈 控 制 的 思想 是 预测 产生 干扰 的 力矩 ,并 把 它 输 入 到 前 馈 通道 中 ,消除 掉 力 矩 。 这 里 
产生 干扰 的 主要 为 重力 项 和 惯性 矩阵 ,但 惯性 矩阵 未 知 , 只 抵消 掉 重 力 项 的 干扰 时 ,同样 把 
输入 力矩 设置 为 20。 
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机 器 人 工具 箱 提供 了 一 个 Simulink 对 puma560 肩 关 节 进 行 前 馈 控 制 的 模型 ,运行 下 
列 命令 : 


>> vloop test2 


加 载 得 到 的 Simulink 模型 如 图 1-45 所 示 。 


扰动 转 矩 输 入 m c ar | 
具有 链接 惯性 的 puma560 
月 关 节 vloop 参 数 


tau di 
前 馈 力 矩 输 入 


图 1-45 前 馈 控 制 Simulink 模型 


运行 仿真 的 结果 如 图 1-46 所 示 。 

4. 位 置 环 

外 层 为 位 置 环 ,负责 维持 位 置 和 确定 关节 的 速度 。 这 里 使 用 简单 的 比例 控制 ,基于 期 望 
位 置 和 实际 位 置 之 间 的 误差 的 比例 控制 器 控制 律 为 : 

4° —-K,(q'—9) (1. 96) 

位 置 控制 环 的 控制 框图 如 图 1-47 所 示 , 其 中 vloop 为 速度 环 模型 ,K， 为 比例 控制 器 的 
增益 。 

机 器 人 工具 箱 提供 了 一 个 对 puma560 肩 关 节 的 位 置 控制 环 进行 测试 的 Simulink fi 
型 ,运行 下 列 命令 : 


>> ploop test 


加 载 得 到 的 Simulink 模型 如 图 1-48 所 示 。 其 中 ,用 一 个 LSPB 轨迹 发 生 器 设置 期 望 的 位 
置 , 它 从 0 到 0.5rad 运动 ,采样 频率 是 1000Hz。 
运行 仿真 图 形 如 图 1-49 所 示 。 
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n " i 


(a) I9] X: Yi ff Rz Ba T f 


T T T T T T T T 


n 


(b) 期 里 关节 和 实际 关节 角 误 差 
* T T T T T T T 
^ 加 加 加 加 十 + * * * 
(©) 反馈 的 得 到 的 电机 力矩 
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1-47 位 置 控制 环 的 控制 框图 
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= 
© 


图 1-49 位 置 控 制 环 的 仿真 图 形 


1.6.4 机 器 人 的 多 关节 控制 


1. 机 器 人 系统 的 伺服 控制 律 
因为 机 器 人 的 操作 辟 具 有 多 个 关节 ,分 别 需要 相应 的 驱动 电机 提供 驱动 力矩 ,并 输出 多 
个 关节 的 位 置 . 速 度 和 加 速度 ,所 以 对 操作 臂 进行 控制 是 一 个 多 输入 多 输出 的 问题 。 
将 控制 律 分 解 为 基于 模型 的 控制 部 分 和 伺服 控制 部 分 ,那么 它 可 以 表示 为 : 
下 一 eF «f (1. 97) 
其 中 ,F、F'.B 为 nX1 的 矢量 ,@ 为 nXn 的 矩阵 。 为 基于 模型 的 控制 部 分 、 而 F falls 
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制 部 分 ,可 以 表示 为 : 
F'— X +K, E -K,E (1. 98) 
其 中 ,K,、K; 为 n X n B KE X nox 1 维 的 位 置 误差 矢量 ,.E 为 nX1 维 的 速度 误差 矢量 。 


2. 基于 模型 的 操作 辟 控 制 
在 前 面 建立 了 机 器 人 的 刚体 动力 学 方程 ,如 下 : 


t= M(g)g+C(g.g)g + G(g)+ Fg) (1. 99) 
其 中 ,M(o)ER" "为 机 器 人 的 惯性 矩阵 ,C(dq,d )ER" 为 科 里 奥 利 矩阵 ,G(d) ER" 表示 重 
力矩 阵 ,F(9 ) 为 摩擦 力矩 。 
可 以 得 到 控制 律 ; 
r=ar +p (1.100) 
其 中 ， 
a—-MG) (2.101) 
B= CG«44-- GG FG) (1.102) 
e’ = ja 4 K,E-- KE (1. 103) 
式 中 : 
E=g—g (1.104) 
3. 前 馈 控制 


使 用 前 馈 控 制 器 的 控制 框图 如 图 1-50 所 示 ,对 机 械 臂 动力 学 模块 的 输入 力矩 为 : 
Q' — Mq" Jä" CQ" ,9°)9° HFE +G" + (IK, (I7 —4)H- K," —90) 
一 9(4 sq" ,9° )- Keg — HK, a —4)0) (1.105) 
HEP KS LEERY ai RE ROSE fü XB ME 开 。 为 速度 增益 (或 者 阻尼 ) 和 矩阵 ,也 为 对 角 和 矩阵 ; 
OC * ) 是 道 动力 学 函数 ,包含 的 3 个 参数 分 别 是 期 望 关节 角 g* 、 期 望 关节 角速度 4“ 、 期 望 关 
节 角 加 速度 4”。 


扭矩 前 馈 


图 1-50 机 器 人 前 馈 控 制 的 控制 框图 
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在 这 里 ,2(9” ,4 7 ,9”) 为 前 馈 力 矩 , 它 提 供 期 望 的 机 械 臂 状态 (9 ,dg ,4” ) 所 需要 的 
关节 力 ; {K。(g “一 g ) 十 Ks (q7 一 g) } 为 反馈 项 , 它 补 偿 控 制 系统 存在 的 误差 ,造成 这 些 误 
差 的 因素 包括 了 惯性 参数 的 不 确定 性 、 未 建 模 力 和 外 部 干扰 。 

机 器 人 工具 箱 中 提供 了 一 个 对 前 馈 控 制 测试 的 Simulink 模型 ,运行 下 面 的 命令 加 载 相 
关 的 Simulink 模型 


>> mdl puma560 
>> sl fforward 


加 载 得 到 的 Simulink 模型 ,运行 结果 如 图 1-51 所 示 ,其 中 ,前 馈 力 矩 是 使 用 RNE 模块 
来 计算 的 ,加 入 到 由 位 置 和 速度 误差 计算 出 来 的 反馈 力矩 中 。 期 望 关节 角度 和 速度 是 使 用 
jtraj 模块 计算 的 。 因 为 机 器 人 结构 改变 相对 缓慢 ,前 馈 力矩 可 以 以 比 误差 反馈 回路 Tn 更 
低 的 速度 Tr 来 估计 。 这 里 采用 零 阶 保持 器 以 维持 相对 较 低 的 采样 率 一 一 20Hz。 


反馈 力矩 
图 1-51 前 馈 控制 的 Simulink 模型 
4. 计算 力矩 控制 
使 用 计算 力矩 控制 器 的 控制 框图 如 图 1-52 所 示 ,对 机 械 臂 动力 学 模块 的 输入 力矩 为 
Q— M(p (d^ KG" — +K, —9))H- CG4Dà- FCD 
= G(q.d. (I^ 十 Ka —0)+ Ko(q* —400) (1. 106) 


1-52 机 器 人 计算 力矩 控制 的 框图 
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其 中 ,K, 是 位 置 增益 矩阵 ,Ks 是 速度 增益 和 矩阵; 9(，。 ) 是 逆 动 力学 函数 。 
机 器 人 工具 箱 中 提供 了 一 个 对 前 馈 控制 测试 的 Simulink 模型 ,运行 下 面 的 命令 加 载 相 
关 的 Simulink BiU. 


>> mdl puma560 
>> p560 = p560.nofriction(); 
2» sl ctorque 


加 载 得 到 的 Simulink 模型 ,运行 结果 如 图 1-53 所 示 。 设 置 jtraj 模块 ,将 初始 关节 角度 
设置 为 qdo=[0 0 0 0 0 0]J, 末 端 关 节 角 度 设置 为 qf=[pi/4 pi/2 一 pi/2 0 0 0]. 
最 大 的 力矩 设置 为 10。 


图 1-53 计算 力矩 控制 的 Simulink 模型 
输入 以 下 命令 ,运行 程序 ， 
>>r = sim('sl ctorque'); 


仿真 开始 时 ,机 器 人 的 操作 臂 按 照 期 望 的 关节 角度 .关节 角速度 和 关节 角 加 速度 运动 ， 
如 图 1-54 所 示 。 


图 1-54 计算 力矩 控制 的 机 器 人 仿真 图 
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此 外 可 以 通过 输入 命令 得 到 关节 和 角度 随时 间 变 化 的 图 形 ,输入 命令 : 


2» t = r.find('tout'); 
>> q = r.find('yout'); 
2» plot(t,q) 


得 到 的 图 形 如 图 1-55 所 示 ,可 以 看 出 ,关节 角度 由 初始 位 置 qd0o=[L0 0 0 0 0 0] 到 末 
端 位 置 df 二 [pi/4 pi/2 —pi/2 0 0 0] 变 化 的 图 形 。 


0 
图 1-55 计算 力矩 控制 的 机 器 人 仿真 图 


1.7 ”其 他 基于 MATLAB 的 机 器 人 工具 箱 


1.7.1 Kuka 控制 工具 箱 (KCT) 的 介绍 与 测试 


1. KCT 的 诞生 背景 

在 过 去 十 年 中 ,MATLAB 已 经 开发 出 了 用 于 机 器 人 系统 建 模 的 几 个 工具 箱 。 这 些 仿 
真 工具 受到 各 种 应 用 场景 的 启发 ,逐渐 开发 出 适用 于 基于 视觉 的 机 器 人 和 空间 机 器 人 的 工 
具 箱 ,实现 了 从 工业 到 学 术 教 育 的 转型 。 其 中 具有 挑战 性 的 问题 是 设计 MATLAB 工具 包 
以 提供 多 功能 和 高 级 编程 环境 用 于 真实 机 器 人 的 运动 控制 。puma560 机 械 手 在 这 个 领域 
已 经 做 了 已 经 做 出 了 一 些 尝 试 。 然 而 ,这 个 机 器 人 有 一 些 已 知 固有 的 软件 限制 ,特别 是 在 实 
时 应 用 程序 方面 。Kuka 控制 工具 箱 (KCT) 在 实时 应 用 程序 方面 取得 突出 进展 ,本 节 将 重 
点 介绍 Kuka 控制 工具 箱 和 其 他 一 些 常 用 的 机 器 人 工具 箱 。 

2. KCT 的 发 展 历程 

Kuka 控制 工具 箱 (KCT) 是 用 于 KUKA 机 器 人 操纵 器 运动 控制 的 MATLAB 函数 集 
合 , 开 发 环境 为 用 户 提供 直观 和 高 级 的 编程 接口 。 该 工具 箱 与 使 用 Eth. RSIXML 的 所 有 
6 自由 度 小 型 和 低 型 库 卡 机 器 人 兼容 .通过 TCP/IP 与 KUKA 控制 器 连接 在 远程 计算 机 上 
运行 。KCT 包括 超过 30 个 功能 ,涵盖 正 向 和 反 向 运动 学 计算 .点 对 点 连接 和 笛 卡 儿 控制 、 
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轨迹 生成 图形 显示 和 诊断 等 操作 。 

库 卡 操纵 器 是 由 世界 领先 的 工业 机 器 人 制造 商 之 一 KUKA 生产 的 一 款 操纵 器 ,其 设 
计 适 用 于 工业 设置 中 的 大 量 应 用 ,例如 组 装 、 材 料 处理 、. 分 配 、 码 翅 和 焊接 任务 。KUKA 已 
经 开发 了 一 种 特定 的 C 语言 编程 语言 , 称 为 KRL( 库 卡 机 器 人 语言 ) ,用 于 机 器 人 运动 控制 。 
这 种 语言 很 简单 ,编程 方便 。 然 而 , 它 不 适用 于 关键 的 实时 远程 控制 应 用 ,不 支持 图 形 界面 
和 高 级 数学 工具 (如 矩阵 运算 、 优 化 和 过 滤 任 务 ) ,不 允许 外 部 硬件 模块 的 简单 集成 ,例如 使 
公共 协议 的 照相 机 或 嵌入 式 设备 USB、PCI 等 。 

为 了 克服 这 些 缺 点 ,MATLAB 工具 箱 Kuka-KRL-tbx 在 KRL 上 构建 MATLAB 抽象 
层 , 使 用 串 行 接口 连接 库 卡 机 器 人 控制 器 (KRC) 和 装载 MATLAB 的 远程 计算 机 。 在 KRC 
上 运行 的 KRL 解释 器 在 机 器 人 和 远程 计算 机 之 间 建 立 双向 通信 ,负责 识别 和 执行 通过 串 
行 接口 传输 的 所 有 指令 。 然 而 , Kuka-KRL-tbx 仍然 受到 一 些 限制 ,例如 工具 箱 的 
MATLAB 命令 与 KRL 函数 是 一 对 一 的 ,阻碍 用 户 设 计 高 级 控制 应 用 程序 , 串 行 接口 可 能 
给 实时 控制 应 用 带 来 限制 。 另 外 ,工具 箱 也 不 包括 用 于 图 形 显示 的 特定 例 程 。 

Kuka 控制 工具 箱 (KCT) 则 很 好 地 解决 了 上 面 的 问题 ,KCT 的 函数 不 像 KRL 命令 那 
样 一 对 一 ,这 使 得 工具 箱 非常 灵活 和 通用 ; KCT 通过 TCP/IP 与 KRC 连接 在 远程 计算 机 
上 运行 ,多 线程 服务 器 在 KRC 上 运行 ,并 通过 Eth. RSIXML( 以 太 网 机 器 人 传感器 接口 
XML) 在 客户 端 与 操纵 器 进行 信息 交换 和 通信 ,这 保证 了 高 传输 速率 ,使 实时 控制 应 用 成 为 
可 能 ; KCT 有 几 个 专用 于 图 形 和 三 维 动画 的 功能 ,并 包括 一 个 图 形 用 户 界面 。 

本 部 分 内 容 将 根据 KCT 工具 箱 中 的 函数 和 KCT: a MATLAB toolbox for motion 
control of KUKA robot mani pulators 一 文 ,对 KCT 的 通信 原理 进行 详解 ,对 所 有 函数 的 功 
能 进行 测试 ,以 让 读者 更 深入 了 解 KCT。 

3. KCT 的 通信 原理 

KCT 与 机 器 人 操纵 器 之 间 的 通信 由 3 个 部 分 组 成 : 在 MATLAB 下 运行 KCT 的 远程 
计算 机 ,KUKA 机 器 人 控制 器 (KRC) ,机 器 人 机 械 手 。 为 了 在 远程 计算 机 和 机 器 人 控制 器 
之 间 建 立 连接 ,KCT 提供 了 kctserver, 它 是 一 个 在 KRC 上 运行 的 C++ 多 线程 服务 器 。 
kctserver 通过 eth. RSIXML( 用 于 TCP/IP-robot 接口 的 KUKA 软件 包 ) 与 kctrsiclient, src 
进行 通信 ,该 kctrsiclient. src 是 在 KRC 上 运行 eth. RSIXML 客户 端 并 管理 与 机 器 人 操纵 
器 的 信息 交换 的 KRL 脚本 。KCT 的 MATLAB 函数 使 用 特定 的 MEX 文件 (默认 选项 ) 或 
通过 MATLAB 仪器 控制 工具 箱 与 ketserver 通信 。 

4. KCT 的 函数 测试 

KCT 工具 箱 共 有 41 个 函数 , 按 功能 可 大 致 分 为 七 类 : 初始 化 .联网 .运动 学 `. 运 动 控 
制 . 图 像 . 齐 次 变换 和 演示 。 

D 初始 化 函数 

初始 化 函数 主要 用 于 工具 箱 和 机 器 人 的 初始 化 设置 ,共有 9 个 函数 ,以 下 将 依次 
介绍 。 

kctrobot() 
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函数 kctrobot() 的 功能 是 显示 所 有 本 工具 箱 支 持 的 库 卡 机 器 人 的 信息 。 本 工具 箱 默 认 
支持 13 款 型 号 的 库 卡 机 器 人 ,其 中 的 信息 均 存 放 在 kctrobotdata. mat 文件 中 。 本 函数 的 程 
序 代码 如 下 所 示 , 从 中 可 见 该 函数 的 程序 是 相当 简单 的 , 先 判 断 能 否 找 kctrobotdata. mat 文 


件 , 如 果 不 能 , 则 打印 


文件 并 打印 13 款 库 卡 机 器 人 版 本 信息 。 


函数 kctrobot() 的 程序 如 下 : 


function kctrobot() 

% Can't read the data structure 

if exist('kctrobotdata.mat') -- 0 
display( 'kctrobotdata.mat not found. '); 
return 

end 

* Reading the data structure 

load kctrobotdata; 

for i = l:size(kctrobotdata, 2) 
disp(['position: ', num2str(i)]); 


kctrobotdata( i) 

end 

运行 结果 : 

>> kctrobot(); position: 3 

position: 1 ans = 

ans = name: 'KR5arc' 
name: 'KR3' linkl: 400 
linkl: 350 link2: 180 
link2: 100 link3: 600 
link3: 265 link4: 120 
link4: 0 link5: 620 
link5: 270 link6: 115 
link6: 75 

position: 4 

position: 2 ans = 

ans = name: 'KR5arcHW' 
name: 'KR5sixxr650" linkl: 400 
linkl: 335 link2: 180 
link2: 75 link3: 600 
link3: 270 link4: 170 
link4: 90 link5: 620 
link5: 295 link6: 200 


link6: 80 


'kctrobotdata. mat not found. ' 信 息 ,如 果 能 , 则 加 载 kctrobotdata. mat 
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kctfindrobot( ) 

FAC kctfindrobot() 的 功能 是 在 kctrobotdata 列表 中 查找 KUKA 机 器 人 的 模型 ,找到 ， 
则 返回 1, 没 找到 , 则 返回 0。 其 用 法 是 : bFound = kctfindrobot(kctname), 其 中 的 参数 
kctname 是 kuka 机 器 人 型 号 名 称 的 字符 串 。 

其 程序 代码 如 下 所 示 : 


function bFound = kctfindrobot(kctname) 

krd = load('kctrobotdata. mat'); 

temp kctrobotdata = krd.kctrobotdata; 

bFound = 0; 

for i = 1:size(temp kctrobotdata, 2) 
if(strcmp(temp kctrobotdata(i).name, kctname) == 1) 

bFound = 1; 

return; 
end 

end 


运行 结果 : 


>> bFound = kctfindrobot( 'KR3') 
bFound = 
H 


文件 中 含有 机 器 人 'KR3', 返 回 1 。 


>> bFound = kctfindrobot( 'KR2') 
bFound = 
0 


文件 中 不 含有 机 器 人 'KR2' ,返回 0。 

kctinsertrobot( ) 

函数 kctinsertrobot() 的 功能 是 将 新 模型 添加 到 kctrobotdata 列表 并 保存 。 其 用 法 是 : 
kctinsertrobot(kctname,kctlinks) ,该 函数 具有 两 个 参数 ,kctname 为 kuka 机 器 人 型 号 名 称 
的 字符 串 ,kctlinks 是 一 个 六 维 向 量 , 它 表示 该 模型 的 六 个 连 杆 的 长 度 。 若 所 添加 的 模型 名 
称 已 存在 , 则 提示 该 模型 已 存在 ; 若 所 添加 的 模型 名 称 未 存在 kctrobotdata. mat 中 , 则 把 新 
模型 存 和 人 kctrobotdata 列表 ,并 提示 新 模型 已 成 功 添 加 。 

函数 kctinsertrobot() 的 程序 如 下 所 示 : 


function kctinsertrobot(kctname, kctlinks) 
if kctfindrobot(kctname) -- 0 

krd = load('kctrobotdata. mat'); 
kctrobotdata = krd.kctrobotdata; 
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kctindex = length(kctrobotdata) +1; 
kctrobotdata(kctindex).name = kctname; 
kctrobotdata(kctindex).linkl = kctlinks(1); 
kctrobotdata(kctindex).link2 kctlinks(2); 
kctrobotdata(kctindex).link3 = kctlinks(3); 
kctrobotdata(kctindex).link4 = kctlinks(4); 
kctrobotdata(kctindex).link5 - kctlinks(5); 
kctrobotdata(kctindex).link6 - kctlinks(6); 
save( 'kctrobotdata. mat', 'kctrobotdata'); 
disp(['Model ',kctnane, ' added correctly. ']) 
else 

disp(['Model ',kctname, ' already in kctrobotdata. mat. ']) 
end 


运行 结果 : 


>> kctlinks = [345, 75, 385, 90, 435, 80]; 
>> kctinsertrobot( 'KR3', kctlinks) 

Model KR3 already in kctrobotdata. mat. 

>> kctinsertrobot( 'KR2',kctlinks) 

Model KR2 added correctly. 


文件 中 已 存在 机 器 人 'KR3', 则 提示 机 器 人 'KR3' 已 存在 ,文件 中 不 存在 机 器 人 'KR2'， 
则 提示 已 成 功 添加 。 

kctdeleterobot() 

函数 kctdeleterobot() 的 功能 是 从 ketrobotdata 列表 中 删除 一 个 特定 的 模型 并 自动 更 
新 kctrobotdata. mat 文件 。 其 用 法 是 : kctdeleterobot (kctname) . 其 中 参数 kctname 是 
kuka 机 器 人 型 号 名 称 的 字符 串 。 若 该 模型 存在 于 kctrobotdata 列表 中 , 则 从 列表 中 删除 该 
模型 ,并 提示 已 成 功 删除 模型 ; 若 该 模型 不 存在 于 kctrobotdata 列表 中 ,提示 未 找到 该 
模型 。 

x& 3 kctdeleterobot ) 的 程序 如 下 所 示 : 


function kctdeleterobot(kctname) 
krd = load('kctrobotdata.mat'); 
temp kctrobotdata = krd.kctrobotdata; 
bDelete - 0; 
for i = 1:size(temp kctrobotdata, 2) 
if(strcmp(temp kctrobotdata(i).name, kctname) -- 1) 
bDelete = 1; 
end 
if (bDelete == 1) && i< size(temp kctrobotdata, 2) 
temp kctrobotdata(i) = temp kctrobotdata(i 1); 
end 
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end 
if bDelete == 1 
kctrobotdata = temp kctrobotdata(1:(size(temp kctrobotdata, 2) —1)); 
save( 'kctrobotdata. mat', 'kctrobotdata'); 
disp(['Model ', kctname, ' removed correctly. ']) 
else 
disp(['Model ', kctname, ' not found. ']) 
end 


运行 结果 : 


>> kctlinks = [345, 75, 385, 90, 435, 80]; 
>> kctinsertrobot( 'KR2',kctlinks) 

Model KR2 added correctly. 

>> kctdeleterobot( 'KR2') 

Model KR2 removed correctly. 

>> kctdeleterobot( 'KRl') 

Model KR1 not found. 


文件 中 存在 机 器 人 'KR2' ,提示 机 器 人 已 成 功 删除 ,文件 中 不 存在 机 器 人 'KR1' ,提示 没 
找到 该 机 器 人 。 

kctinit() 

函数 kctinit() 的 功能 是 提取 某 一 特定 KUKA 机 器 人 的 数据 并 将 其 保存 在 . mat 文件 
中 ,这 些 机 器 人 数据 是 运动 功能 操作 所 必需 的 ,另外 该 函数 还 可 以 加 载 TCP/IP 通信 选项 。 
其 用 法 为 : kctinit(robotname) ,其 中 参数 robotname 为 机 器 人 型 号 名 称 字符 串 。 若 参数 为 
空 , 则 提示 错误 ,需要 输入 一 个 参数 ; 若 输入 的 参数 模型 不 存在 , 则 提示 错误 ; 若 模型 存在 ， 
则 提取 该 模型 的 数据 保存 到 ketrobotlinks. mat 文件 中 ,并 加 载 kcttcpiptype. mat 文件 。 

函数 的 程序 如 下 所 示 : 


function kctinit(robotname) 
if(nargin ~= 1) 
display(' ERROR : kctinit requires one input. '); 
return; 
end 
krd = load('kctrobotdata'); 
i=1; 
while — strcmp(robotname, krd. kctrobotdata( i). name) 
isi+t1; 
end 
kctrobotlinks = [krd. kctrobotdata( i). linkl, krd. kctrobotdata( i). link2, krd. kctrobotdata( i). 
link3, krd. kctrobotdata( i). link4, krd. kctrobotdata( i). link5, krd. kctrobotdata( i). link6]; 
save( 'kctrobotlinks.mat', 'kctrobotlinks'); 
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kctchframe(eye(4)) ; 

% Load TCP/IP comunication type 
global kcttcpiptype; 
load('kcttcpiptype. mat'); 


运行 结果 : 
2» kctinit() 

ERROR : kctinit requires one input. 
>> kctinit( 'KR2') 
Index exceeds matrix dimensions. 
Error in kctinit (line 43) 


while —strcmp(robotname, krd. kctrobotdata( i). name) 
>> kctinit('KR3') 


kctsetbound( ) 

函数 kctsetbound () 的 功能 是 设置 机 器 人 的 工作 区 边界 ,并 将 工作 区 边界 保存 到 
kctrobotbound. mat 文件 中 。 其 用 法 为 : ketsetbound C bound) ,其 中 参数 bound 为 2X6 的 
矩阵 ,第 一 行为 X,Y,Z 轴 坐 标的 最 大 和 最 小 值 第 二 行为 关节 角度 4,5,6 的 最 大 和 最 小 值 。 
设置 好 参数 后 调用 函数 , 则 将 工作 区 边界 保存 到 kctrobotbound. mat 文件 中 ,并 调用 
kctgetboundO PR. 

函数 kctsetbound() 的 程序 如 下 : 


function kctsetbound(kctworkspace) 

save( 'kctrobotbound. mat', 'kctworkspace') ; 
kctgetbound() ; 

运行 结 AX H 


>> kctsetbound([ - 800 800 — 800 800 0 800; - 90 180 -90 180 -90 180]) 


调用 该 函数 后 弹出 一 个 用 户 界面 ,如 图 1-56 所 示 , 其 绘 出 了 机 器 人 和 机 器 人 的 工作 区 间 。 


1-56 ”函数 kctsetbound() 生 成 的 KUKA 机 器 人 及 其 工作 区 间 
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kctgetbound() 

函数 kctgetbound() 的 功能 是 显示 机 器 人 的 工作 区 边界 。 其 用 法 是 : B — kctgetbound()， 
其 返回 值 为 包含 边界 的 最 小 值 和 最 大 值 的 2X6 矩阵。 调用 函数 后 返回 机 器 人 的 工作 区 边 
界 数据 ,并 调用 函数 kctdrawbound() 和 kctdisprobot() 将 工作 区 边界 和 机 器 人 以 三 维 图 形 
的 形式 表示 出 来 。 

函数 kctgetbound() 的 程序 为 : 


function kctworkspace = kctgetbound 

kw = load('kctrobotbound'); 
kctworkspace = kw.kctworkspace; 

h bound - figure(); 

kctrobothome = load('kctrobothome.mat'); 


df - 40; 
* Box color 
color2 - [0.0,0.0,0.8]; 
axis equal; 
k= [0:pi/4:2 * pi]; 
d- 40; 


Xzvert = [d* 0.25 * sin(k) ;d * 0.25 * sin(k + pi/4) ;zeros(1, length(k) ) ]; 
Yzvert = [d*0.25 * cos(k) ;d * 0.25 * cos(k + pi/4) ;zeros(1, length(k) ) ]; 
Zzvert = [zeros(1,length(k)) ;zeros(1, length(k) ) ;d/2 x ones(1, length(k))]; 
Xyvert = [d*0.25 x» sin(k);d* 0.25 * sin(k + pi/4) ;zeros(1, length(k))]; 
Yyvert = [zeros(1,length(k));zeros(1,length(k));d/2 * ones(1, length(k))]; 
Zyvert = [d*0.25 * cos(k);d * 0.25 x cos(k + pi/4) ;zeros(1, length(k) ) ]; 
Xxvert = [zeros(1,length(k));zeros(1, length(k) ) ;d/2 x ones(1, length(k))]; 
Yxvert = [d* 0.25 * cos(k) ;d * 0.25 * cos(k + pi/4) ;zeros(1, length(k) ) ]; 
Zxvert = [d*0.25 * sin(k) ;d * 0.25 x sin(k + pi/4) ;zeros(1, length(k))]; 
framecolor = 'red'; 
patch(df * Xxvert, Yxvert, Zxvert, framecolor); 
patch(Xyvert, df * Yyvert, Zyvert, framecolor); 
patch(Xzvert, Yzvert, df + Zzvert, framecolor); 
line([0 d£], [0 0], [0 0], 'color', framecolor, 'LineWidth',2); 
line([O 0], [0 d£], [0 0], 'color', framecolor, 'LineWidth',2); 
line([0 0], [0 0], [0 d£], 'color', framecolor, 'LineWidth',2); 
text(df + 5,5,5, 'X0'); 
text(5,df + 5,5, 'Y0"); 
text(5,5,df * 5, 'Z0"); 
grid on; 
axis equal; 
hold on; 

* Plot robot's bounds 
kctdrawbound(kctworkspace, color2); 

* Plot the robot 
kctdisprobot( — kctrobothome. kcthomeposition(2,:),h bound); 
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运行 结果 : 


>> kctsetbound([ - 800 800 -800 800 0 800; - 90 180 -90 180 -90 180]) 
>> kctsetbound([ ~ 800 800 — 800 800 0 800; - 90 180 -90 180 -90180]) 
>> kctgetbound() 
ans = 

- 800 800 -800 800 0 800 

-90 180 -90 180 -90 180 
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所 示 , 其 绘 出 了 机 器 人 和 机 器 人 的 工作 区 间 。 
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图 1-57 函数 kctgetbound() 生 成 的 KUKA 机 器 人 及 其 工作 区 间 


kctchecksystem() 

函数 kctchecksystem O 的 功能 是 检查 系统 是 否 满足 要 求 。 其 用 法 为 : [issystemok, 
foundmatlab,foundtoolbox] = kctchecksystem()。 其 中 返回 值 issystemok 为 1 ,表示 系统 
支持 该 工具 箱 ; 为 0, 表 示 系 统 不 支持 该 工具 箱 。 返 回 值 foundmatlab 为 1, 表 示 MATLAB 
支持 该 工具 箱 (MATLAB7. 0 及 以 上 版 本 ); 为 0, 表示 MATLAB 不 支持 该 工具 箱 。 返 回 
值 foundtoolbox 为 1, 表 示 工 具 箱 已 成 功 安装 ; 为 0, 表 示 工 具 箱 为 成 功 安装 。 另 外 ,函数 也 
会 返回 MATLAB 的 版 本 信息 ,车 MATLAB 版 本 过 低 或 工具 箱 未 正常 安装 , 则 做 出 相应 的 
提示 。 

函数 kctchecksystem() 的 程序 如 下 : 


function [issystemok, foundmatlab, foundtoolbox] = kctchecksystem() 
global kcttcpiptype; 
load('kcttcpiptype. mat'); 
toolboxlist = ver(); 
% Check Matlab version 
matlabversion = version(); 
foundmatlab - 0; 
if matlabversion(1) >= 7 
foundmatlab - 1; 
fprintf('Matlab version $% s.\n', matlabversion); 
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end 
* Check if Instrument Control Toolbox is installed 
foundtoolbox - 0; 
fori - 1:size(toolboxlist,2) 
if strcmp(toolboxlist(i).Name, 'Instrument Control Toolbox') == 1 
foundtoolbox - 1; 
display(' Instrument Control Toolbox found. '); 
break; 
end 
end 
if foundmatlab -- 0 
display('Matlab version too old required 7.x or higher. '); 
display('Some KCT functions may not work properly. '); 
end 
if foundtoolbox -- 0 
display('Instrument Control Toolbox not found.'); 
display('MEX functions will be used for the network communication. '); 
kctsettcpip( 'MEX'); 
end 
issystemok - and(foundmatlab, foundtoolbox); 


运行 结果 : 


>> [issystemok, foundmatlab, foundtoolbox] = kctchecksystem() 
Matlab version 8.0.0.783 (R2012b). 
Instrument Control Toolbox found. 
issystemok = 1 
foundmatlab - 1 
foundtoolbox - 1 


kctdrawbound( ) 

函数 kctdrawbound() 的 功能 是 显示 机 器 人 边界 。 其 用 法 为 : ketdrawbound(ketworkspace . 
kctcolor,h_fig) ,其 中 参数 kctworkspace 为 包含 机 器 人 边界 的 矩阵 ,kctcolor 为 边框 颜色 
(可 选 ),h_fig 为 图 的 句柄 (可 选 ) 。 

运行 结果 : 


>> kctdrawbound([ — 600 600 — 600 600 0 600]) 


输入 该 函数 后 弹出 一 个 用 户 界面 ,如 图 1-58 所 示 , 中 间 较 深 色 的 箱子 表示 机 器 人 的 
边界 。 

2) 联网 函数 

联网 函数 主要 用 于 通信 连接 ,共有 4 个 函数 ,如 下 所 示 : 

ketsettcpip() 
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Xx 
图 1-58 函数 kctdrawbound() 生 成 的 KUKA 机 器 人 及 其 工作 区 间 


PKŠ ketsettcpip() 的 功能 是 设置 网 络 通信 协议 (仪器 控制 工具 箱 或 Mex 文件 )。 其 用 
法 为 : kctsettcpip('ICT') 或 kcetsettepip(" MEX) ,其 中 kctsettcpip('ICT') 对 应 于 仪器 控制 
工具 箱 ,kctsettcpip('MEX') 对 应 于 Mex 文件 ,参数 value 为 表示 TCP/IP 类 型 的 字符 串 。 
若 调用 函数 时 没有 设 定 参数 , 则 提示 错误 并 要 求 输入 一 个 参数 ; 若 调 用 函数 时 设 定 参数 为 
ICT, 则 调用 kctchecksystem() 函 数 判 断 工具 箱 是 否 能 用 于 TCP/IP 通信 ; 调用 函数 时 设 定 
参数 为 ICT 或 MEX 均 会 关闭 当前 的 TCP/IP 通信 形式 ,并 设置 为 新 的 TCP/IP 通信 形式 ， 
同时 将 新 的 TCP/IP 通信 类 型 保存 至 kcttcpiptype. mat 文件 中 。 

函数 kctsettcpip() 的 程序 如 下 : 


function kctsettcpip(value) 
global kctipvar 
if(nargin ~= 1) 
display(' ERROR : kctsettcpip requires one input. '); 
return; 
end 
* Load TCP/IP comunication type 
global kcttcpiptype; 
if value == 'ICT' 
[issystemok, foundmatlab, foundtoolbox] = kctchecksystem(); 
if foundtoolbox -- 0 
display(' Instrument Control Toolbox cannot be used for TCPIP communication. '); 
return; 
end 
end 
% Closing previous communication if exist 
if isempty(kctipvar) == 0 
display('Closing previous TCPIP communication. '); 
kctcloseclient(); 
end 
% Changing TCPIP communication type 
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kcttcpiptype = value; 
save( 'kcttcpiptype.mat', 'kcttcpiptype'); 


运行 结果 : 


2» kctsettcpip('ICT') 
Matlab version 8.0.0.783 (R2012b). 
Instrument Control Toolbox found. 
Closing previous TCPIP communication. 
KUKA ICT client succesfully closed. 
>> kctsettcpip( 'MEX') 


kctgettcpip() 

函数 kctgettcpip() 的 功能 是 获取 网 络 通信 协议 ,其 用 法 是 : ketgettepipO ,其 返回 信息 
为 网 络 通信 类 型 。 若 通信 类 型 为 ICT, 则 打印 KUKA communication protocol: Instrument 
Control Toolbox. ; 若 通信 类 型 为 MEX, 则 打印 KUKA communication protocol; MEX files. 。 

函数 kctgettcpip() 的 程序 如 下 : 


function kctsettcpip(value) 
% Load TCP/IP comunication type 
global kcttcpiptype; 
if kcttcpiptype == 'ICT' 
display(' KUKA communication protocol: Instrument Control Toolbox. '); 
elseif kcttcpiptype == 'MEX' 
display(' KUKA communication protocol: MEX files. '); 
end 


运行 结果 : 


>> kctgettcpip() 
KUKA communication protocol: Instrument Control Toolbox. 


kctclient C) 
函数 kctclient() 的 功能 是 初始 化 KUKA 机 器 人 通信 的 MATLAB 客户 端 , 它 可 以 初始 
化 指定 的 URL 和 端口 上 的 TCP/IP 连接 ,以 便 启动 与 KUKA 机 器 人 的 服务 器 的 通信 。 其 
用 法 为 : t ketclient(str address) ,其 中 参数 str. address 为 服务 器 PC 的 IP 地 址 (字符 串 
格式 ) ,参数 SampleTime 为 控制 通信 所 需 的 时 间 ( 最 小 值 为 0. 0155) ,返回 值 t 为 与 KUKA 
机 器 人 通信 的 处 理 程序 。 
运行 结果 : 
2» t = kctclient('192.168.1.0') 
KCTCLIENT: connect the robot with MATLAB 
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Start the kctserver. exe on the KUKA robot controller and press any key to continue .-- 
Connection on port failed 
Frame position and orientation 
kctptfr - 
0 0 0 
TCPIP Object : TCPIP-192.168.1.0 
Communication Settings 


RemotePort: 2999 
RemoteHost: 192.168.1.0 
Terminator: 'LF' 
NetworkRole: client 
Communication State 
Status: closed 
RecordStatus: off 
Read/Write State 
TransferStatus: idle 
BytesAvailable: 0 
ValuesReceived: 0 
ValuesSent: 0 
kctrotfr = 
1 0 0 
0 1 0 
0 0 1 
Sample time for communication 
Tem 
0.1500 


kctcloseclient( ) 
函数 kctcloseclient() 的 功能 是 终止 用 于 KUKA 机 器 人 通信 的 MATLAB 客户 端 , 即 终 
tj KUKA 机 器 人 服务 器 的 TCP/IP 连接 。 其 用 法 是 : kctcloseclient(t) ,其 中 参数 t 为 与 
KUKA 机 器 人 通信 的 处 理 程序 。 
kctcloseclient( ) 的 程序 如 下 : 


function kctcloseclient() 
global kctipvar; 
* Load TCP/IP comunication type 
global kcttcpiptype; 
if( kcttcpiptype == 'ICT'& ~ isempty(kctipvar) ) 
fclose(kctipvar); 
clear global kctipvar; 
clear global kctrobotlinks; 
fprintf('KUKA ICT client succesfully closed. \n') 
elseif( kcttcpiptype == 'MEX'& —isempty(kctipvar) ) 
kctcloseclientmex(); 
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clear global kctipvar; 

clear global kctrobotlinks; 

fprintf('KUKA MEX client succesfully closed. \n') 
else 

fprintf('TCPIP communication not initialized. Wn') 
end 


运行 结果 : 


>> kctcloseclient() 
KUKA ICT client succesfully closed. 


3) 运动 学 函数 

运动 学 函数 主要 用 于 求 取 机 器 人 的 运动 学 ,共有 5 个 函数 ,如 下 所 示 : 

kctreadstate( ) 

函数 kctreadstate() 的 功能 是 返回 机 器 人 的 当前 状态 ,该 函数 返回 一 个 2X6 的 矩阵 ,第 
一 行为 实际 位 置 ,第 二 行为 机 械 臂 的 关节 角度 。 其 用 法 为 : kctreadstate() ,返回 值 为 表示 机 
器 人 状态 的 2X6 和 矩阵 ,第 一 行为 末端 效应 器 的 位 置 , 第 二 行为 机 械 臂 关节 连接 处 的 角度 。 

函数 kctreadstate() 的 程序 如 下 : 


function robotstate = kctreadstate() 
global Ts; 
global kctipvar; 
global kcttcpiptype; 
t = kctipvar; 

tic; 

% Reading Data 

v coor = []; 

v axis = []; 

if kcttcpiptype == 'ICT' 
fwrite(t, '< ask/2') ; 
B = t.BytesAvailable; 
while t.BytesAvailable < 400 
end 
DataReceived = char(fread(t,400)); 
DataReceived - DataReceived'; 

elseif kcttcpiptype == ' B 
totalBytesRec - 0; 
DataReceived = ''; 
kctsenddatamex('« ask/»') ; 
[bytesRec, dataRec] = kctrecdatamex(); 
dataRec = dataRec([1:bytesRec]); 
DataReceived([1:bytesRec]) - char(dataRec); 
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运行 结果 : 
由 于 没有 连接 硬件 ,不 能 返回 机 器 人 的 当前 状态 。 
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>> kctreadstate() 
Error using icinterface/fwrite (line 191) 
OBJ must be connected to the hardware with FOPEN. 
Error in kctreadstate (line 51) 
fwrite(t, '<ask/>'); 


kctfkine() 

函数 kctfkine() 的 功能 是 计算 机 器 人 的 正 向 运动 学 ,并 返回 机 器 人 正 向 运动 学 的 齐 次 
矩阵 (本 工具 箱 利用 DH 法 求 正 向 运动 学 )。 其 用 法 为 : H_0e = kctfkine(angleDH), 其 中 
参数 angleDH 为 以 角度 为 单位 的 关节 角 矢量 ,返回 值 H_0e 为 正 向 运动 学 的 齐 次 矩阵 。 

工业 机 器 人 的 正 向 运动 学 指 已 知 机 器 人 各 关节 类 型 相 邻 关节 之 间 的 尺寸 和 相 邻 关节 
相对 运动 量 的 大 小 时 ,确定 机 器 人 末端 执行 器 在 固定 坐标 系 中 的 位 姿 。 

KUKA 机 器 人 为 六 自由 度 机 器 人 ,可 以 通过 每 个 连接 点 的 相对 位 姿 求 得 最 终 的 位 姿 。 
不 妨 设 六 个 关节 角 分 别 为 01 ,02,03 04 05 406 ,六 个 连 杆 长 度 分 别 为 41 ,2.13,04,15,76, 令 和 T 
表示 从 0 位 置 到 1 位 置 的 刚性 运动 齐 次 变换 矩阵 , 则 有 

T 一 
xk kct fkine() 的 程序 如 下 : 


function H 0e = kctfkine(angleDH) 

global kctrobotlinks; 

global kctptfr; 

global kctrotfr; 

l1 = kctrobotlinks(1); 

12 = kctrobotlinks(2); 

13 = kctrobotlinks(3); 

14 - kctrobotlinks(4); 

15 - kctrobotlinks(5); 

l6 = kctrobotlinks(6); 

a = angleDH; 

H 01 = kctrotoz(a(1)) * kcttran([0,0,11]); 

H 12 = kctrotox(90) x kcttran([12,0,0]); 

H 23 = kctrotoz(a(2)) * kcttran([13,0,0]); 

H 34 = kctrotoz(a(3)) * kcttran([sart(15^2 + 14 ^2),0,0]) * kctrotoy(90); 
H 45 = kctrotoz(a(4)) * kctrotoy( - 90); 

H 56 - kctrotoz(a(5)) * kctrotoy(90); 

H 6e = kctrotoz(a(6)) * kcttran([0,0,16]) x kctrotoz( - 90); 
Hoe = H01xH 12xH 23xH 34xH 45x H 56x H 6e; 

H 0e(1:3,1:3) = inv(kctrotfr(1:3,1:3)) * H 0e(1:3,1:3); 
temp fr = [kctrotfr(1:3,1:3), kctptfr'; [0 0 0 1]]; 

temp fr = inv(temp fr); 

H 0e(1:3,4) = temp fr(1:3,1:3) x*H 0e(1:3,4) + temp fr(1:3,4); 
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分 析 以 上 程序 可 知 ,该 函数 利用 式 3-2-1 一 3-2-8 求 取 机 器 人 相对 于 基本 坐标 系 的 齐 次 
给 阵 , 再 将 其 变换 到 用 户 自 定义 的 坐标 系 ( 当 前 坐标 系 ), 得 到 最 终 的 齐 次 矩阵 。 
运行 结果 : 


>> angleDH = [30,30, 30, — 30, 30,30] 
angleDH = 
30 30 30 -30 30 30 
>> H 0e = kctfkine(angleDH) 
He = 
0.8331 -0.5480 -0.0748 396.6590 
0.5480 0.7996 0.2455 250.6618 
-0.0748 -0.2455 0.9665 788.8148 
0 0 0 1.0000 


kctikine() 

函数 kctikine O ff] 27] fie Je 3 [el BL 8 A. 0] 3 3 29] ^7- . BITE EDLE AI 39 d 2 ^ 3 Ind DA ff 
度 为 单位 的 关节 角 向 量 。 其 用 法 为 : angleDH = kctikine(mat), 其 中 参数 mat 为 一 个 4X4 
的 齐 次 矩阵 ,返回 值 angleDH 为 以 度 为 单位 的 关节 角 矢 量 。 

给 定 机 器 人 的 终端 位 姿 , 求 各 关节 变量 , 称 为 运动 学 逆 解 。 

函数 kctikine() 的 程序 如 下 所 示 : 


function angleDH = kctikine(mat) 
global kctrobotlinks; 
global kctptfr 
global kctrotfr 
11 = kctrobotlinks(1); 
12 = kctrobotlinks(2); 
13 = kctrobotlinks(3); 
14 = kctrobotlinks( 4); 
15 = kctrobotlinks(5); 
16 = kctrobotlinks(6); 
vet = mat(1:3,4); 
rot = mat(1:3,1:3); 
delta = 1; 
gamma = 1; 
% update the new frame (if it exists) 
vet = kctptfr' + kctrotfr(1:3,1:3) * vet; 
rot = kctrotfr(1:3,1:3) * mat(1:3,1:3); 
if vet(1,1)«0 

delta = 0; 

if vet(2,1)«0 

gamma = 0; 

end 

end 
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* Computes the coordinates of joint 5 using joint 6. 

vet = vet- 16 * rot(1:3,3); 

theta 1 - asin(vet(2,1)/(sqrt(vet(2,1)^2 * vet(1,1)^2))); 

theta 1 = ((1- delta) * (pi- theta 1) + theta 1 * delta) * (gamma) + ((1— delta) « (— pi- 
theta 1)- theta 1 x delta) * (1 - gamma); 

t2 1 = atan((vet(3,1) - 11)/(sqrt(vet(1,1)^2- vet(2,1)^2) - sqrt((12 * sin(theta 1))^2 + (12 
* cos(theta 1))^2))); 

d 2 5 = sqrt((vet(3,1) - 11)^2 * (sqrt(vet(1,1)^2- vet(2,1)^2) - sqrt((12 * sin(theta 1))^2 
+ (12 * cos(theta 1))^2))^2); % distanza dal giunto 2 5 

t2 2 = acos(((14 ^2 + 15^2) -13^2-d 2 5^2)/(-2*13*d 2 5)); % Carnot inverse formula 

t3 1 = acos((13^2- (14^2* 15^2) -d 2 5^2)/( -2 sart(14^2* 15^2) «d 2 5)); 

theta 2 - t2 2*t2 1; 

theta 3 253) x (£3514:£2*2) ; 

* Computes the forward kinematics between joint 0 and joint 3 

H 01 = kctrotoz(theta 1 * 180/pi) x kcttran([0,0,11]); 


H 12 = kctrotox(90) * kcttran([12,0,0]); 
H 23 = kctrotoz(theta 2 * 180/pi) * kcttran([13,0,0]) ; 
H 34 = kctrotoz(theta 3 * 180/pi) x kcttran([sqrt(15^2 + 14 ^2),0,0]) * kctrotoy(90); 


H03-HOlxH12xH 23xH 34; 
HO03 = HO 3(1:3,1:3); 
* Indirect computation of the homogeneous matrix between joint 4 and 6 
H4 e = inv(H 0 3) x rot; 
if rot(3,3)«0 && rot(1,2)» 7 0 && rot(3,2)» - 0 
theta 4- - atan(H4 e(1,3)/H4 e(2,3)); 
theta 5 = asin(H4 e(2,3)/cos(theta 4)); 
theta 6= —asin(H4 e(3,2)/sin(theta 5)); 
elseif (rot(3,3)«0 && rot(1,2)«0) || (rot(3,3)«0 && rot(3,2)« 0) 
theta 47 - atan2(H4 e(1,3),H4 e(2,3)); 
theta 5 = asin(H4 e(2,3)/cos(theta 4)); 
theta 6-7 - atan2(H4 e(3,2),H4 e(3,1)); 
elseif rot(3,3)> 0 
theta 4= - atan2(H4 e(1,3),H4 e(2,3)); 
theta 67 - atan2(H4 e(3,2),H4 e(3,1)); 
theta 57 atan2((H4 e(3,1)/cos(theta 6)), (H4 e(3,3))); & 
end 
angleDH = [theta 1 theta 2 theta 3 theta 4 theta 5 theta 6] * 180/pi; 


分 析 以 上 程序 可 知 ,该 函数 先 判 断 当 前 坐标 系 是 否 为 用 户 自 定义 坐标 系 , 若 是 , 则 进行 
相应 的 坐标 变换 ,然后 逆向 求解 出 6 个 关节 角 。 
运行 结果 : 


>mat=[100100;01010;001100;0001] 


mat = 
1 0 0 100 
0 1 0 10 
0 0 1 100 
0 0 0 St 
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2» angleDH = kctikine(mat) 
angleDH = 
5.7106 一 36.6159 -105.1919 180.0000 


kctfkinerpy() 
函数 kctfkinerpy() 的 功能 是 计算 机 器 人 的 了 


128. 1922 174.2894 


E 向 运动 学 (返回 姿势 ), 即 返回 J 


E 向 运动 学 


向 量 p = [zx,y,z,roll,pitch,yawj]。 其 用 法 为 : p = kctfkinerpy (angleDH), 其 中 参数 
angleDH 为 以 度 为 单位 的 关节 角 矢 量 ,返回 值 p 为 六 维 向 量 : 前 三 个 分 量 是 末端 执行 器 的 


zy 坐标 ,最 后 三 个 分 量 是 末端 执行 器 的 侧 倾 - 
函数 kctfkinerpy() 的 程序 如 下 所 示 : 


function [c rpy] = kctfkinerpy(angleDH) 
H = kctfkine(angleDH); 
coor = H(1:3,4); 
Y = real(atan(H(2,1)/H(1,1))); 
P = real( - asin(H(3,1))); 
Rx = inv(kctrotoz(Y) * kctrotoy(P)) x H; 
Rtemp(1,1) = real(acos(Rx(2,2))); 
Rtemp(2,1) 7 real(asin(Rx(3,2))); 
Rtemp(3,1) = real( -asin(Rx(2,3))); 
Rtemp(4,1) = real(acos(Rx(3,3))); 
% Disambiguate the solution 
for i = 1:4 


俯仰 - 偏 航 角 ( 以 度 为 单位 ) 。 


Hdiff =H - kctrotoz(Y) * kctrotoy(P) * kctrotox(Rtemp(i,1)); 


kctsignal = 0; 


for j = 1:3 
fork = 1:3 
if abs(Hdiff(j,k))» 0.0001 
k= 4; 
j= 4; 
kctsignal = 1; 
end 
end 
end 
if kctsignal == 
R = Rtemp(i,1); 
i=5; 
end 
end 
try 


c_rpy= [coor',Y » 180/pi,P x 180/pi,R x 180/pi]; 


catch 


warning('Solution may be unfeseable, please check joint angles 4,5,6') 


end 
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分 析 以 上 程序 可 知 ,该 函数 调用 函数 kctfkine() 求 取 正 向 运动 学 的 齐 次 矩阵 解 ,再 将 齐 
次 和 矩阵 经 过 变换 转化 为 六 维 向 量 卫 一 [Lz,yvz'roll,pitch,yaw]。 
roll pitch, yaw 角 可 以 通过 以 下 公式 求 取 : 
cosy + cosp cosy* sinp* sinr — siny * cosr cosy » sinp * cosr sinr * siny 


siny + cosp siny • sinp • sinr + cosy * cosr siny • sinp * cosr — cosy » sinr 


— sinp cosp * sinr cosp * cosr 
ru nz rs 
= f T22 | 
DNE: rs 
运行 结果 


>> angleDH =[5.7106 -36.6159 -105.1919 180.0000 128.1922 174.2894] 
angleDH = 
5.7106  - 36.6159 -105.1919 180.0000 128.1922 174.2894 
2» p = kctfkinerpy(angleDH) 
p= 
99.9999 10.0000 100.0000 0.0000 0.0000 0 


kctikinerpy ©) 

函数 Kctikinerpy O (19 3J] fie Je: JA 3 Hh YE E BL d A D 36 de z^. BAA 6 维 向 量 p = 
[zyy's'roll,pitch,yaw] 计 算 机 器 人 的 逆 运 动 学 。 其 用 法 为 : angleDH = kctikine(p), 其 中 参 
数 p 为 六 维 向 量 : 前 三 个 分 量 是 末端 执行 器 的 +、y、z 坐标 ,后 三 个 分 量 是 末端 执行 器 的 侧 
倾 -俯仰 - 偏 航 角 ( 以 度 为 单位 ) ,返回 值 angleDH 为 以 度 为 单位 的 关节 角 矢量 。 

函数 kctikinerpy() 的 程序 如 下 : 


function angDH = kctikinerpy(c rpy) 
A = c rpy(1,4); 
B = c rpy(1,5); 
C = c rpy(1,6); 
rot = kctrotoz(A) * kctrotoy(B) * kctrotox(C) 
M = [rot(1:3,1:3),c rpy(1:3)';000 1]; 
angDH = kctikine(M); 


分 析 以 上 程序 可 知 ,该 函数 利用 旋转 变换 : 


1 0 0 
R.G» = i cosó s 
0 sing cosh. 
cosü 0 sing 

R,()=| 0 1 0 


— sin 0 cosh. 
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cosÜü 一 sin0 0] 
R:(0) = E cos 1 
0 0 1 

R = Rz(roll) * RyCpitch) * RxCyaw)3-5-4 

实现 六 维 向 量 p = [zyvz:roll,pitch,yawj] 与 齐 次 矩阵 之 间 的 变换 ,再 调用 函数 
kctikine() 求 取 机 器 人 的 逆 运 动 学 。 
运行 结果 : 
p= 
50 60 80 30 60 45 

>> angleDH = kctikinerpy(p) 


rot = 
0.4330 0.1768 0.8839 0 
0.2500 0.9186  - 0.3062 0 
— 0.8660 0.3536 0.3536 0 
0 0 0 1.0000 
angleDH - 


78.8904 144.1135 - 112.5877 68.2578 85.7598 -99.9667 


4) 运动 控制 函数 

运动 控制 函数 主要 用 于 控制 机 器 人 的 运动 ,共有 10 个 函数 ,如 下 所 示 : 

ketsetjoint ©) 

函数 kctsetjoint() 的 功能 是 将 关节 角度 设置 为 所 需 的 值 , 即 将 机 器 人 从 当前 配置 移动 
到 所 需 的 配置 ,并 向 机 器 人 发 送 速度 曲线 。 其 用 法 为 : [robotinfo, warn] = kctsetjoint 
(angleDH,vp,option) ,参数 angleDH 为 关节 角 矢 量 , vp 为 所 支持 的 最 大 速度 百分比 ， 
option 为 指定 其 他 属性 参数 对 。 例 如 ,'Plot ' ,布尔 变量 (1/0) 可 以 启用 /禁用 绘图 ; 
'FrameDimension', float 变量 可 以 设置 未 端 效应 器 框架 尺寸 ; FrameStep'. float 变量 可 以 
设置 末端 效应 器 框架 步 长 : 'FrameColor' ,可 以 确定 末端 效应 器 框架 的 颜色 ; ' TrajColor'. 
可 以 确定 机 器 人 轨迹 的 颜色 ; 'ShowRobot' ,布尔 变量 (1/0) 可 以 启用 /禁用 机 器 人 的 绘图 ， 
'View' ,可 以 设 定 视点 规格 。 返 回 值 robotinfo 为 包含 了 机 器 人 关节 角度 序列 的 矩阵 , warn 
为 二 进 制 变量 ,如果 warn = 1, 则 发 生 问题 。 

运行 结果 : 

>> [robotinfo, warn] = kctsetjoint([90 20 45 30 45 30],30, 'Plot',1) 

Error using icinterface/fwrite (line 191) 

OBJ must be connected to the hardware with FOPEN. 

Error in kctreadstate (line 51) 

fwrite(t, '< ask/»') ; 
Error in kctsetjoint (line 111) 
robotstate = kctreadstate(); 
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由 于 没有 连接 到 硬件 ,不 能 得 到 最 终 的 测试 结果 。 

kctsetxyz() 

函数 kctsetxyz() 的 功能 是 将 末端 执行 器 移动 到 所 需 位 置 , 即 将 机 器 人 的 末端 执行 器 从 
当前 位 置 移动 到 所 需 的 位 姿 Lx,y,z,roll,pitch,yawj, 并 使 用 kctmovejoint 向 机 器 人 发 送 速 
度 剖 面 。 其 用 法 为 : [robotinfo.warn] = kctsetxyz(pose,vp，option), 其 中 参数 pose 为 期 
望 的 末端 效应 器 姿态 的 矢量 Lx,y,z'roll,pitch,yawj,vp 为 所 支持 的 最 大 速度 百分比 ， 
option 为 指定 其 他 属性 参数 对 。 例如, 'Plot ' ,布尔 变量 (1/0) 可 以 启用 /禁用 绘图 ; 
'FrameDimension',float 变量 可 以 设置 末端 效应 器 框架 尺寸 ; ' FrameStep' . float 变量 可 以 
设置 末端 效应 器 框架 步 长 ; 'FrameColor', 可 以 确定 末端 效应 器 框架 的 颜色 ;'TrajColor'， 
可 以 确定 机 器 人 轨迹 的 颜色 ;'ShowRobot', 布 尔 变量 (1/0) 可 以 启用 /禁用 机 器 人 的 绘图 ; 
"View', 可 以 设 定 视点 规格 。 返 回 值 robotinfo 为 包含 机 器 人 关节 角度 序列 的 矩阵 ,warn 为 
二 进 制 变量 ,如 果 warn = 1, 则 发 生 问 题 。 

运行 结果 : 


>> [robotinfo, warn] = kctsetxyz([400 200 400 45 60 90],30, 'Plot',1) 
Error using fwrite 
Invalid file identifier. Use fopen to generate a valid file identifier. 
Error in kctreadstate (line 51) 

fwrite(t, '< ask/»'); 
Error in kctsetxyz (line 64) 
currentpos = kctreadstate(); 


由 于 该 函数 调用 了 ketreadstate OO 函数 ,而 kctreadstate O 函数 需要 连接 硬件 , 故 没有 得 
到 想 要 的 测试 结果 。 

kctmovejoint() 

函数 kctmovejoint() 的 功能 是 将 关节 速度 设置 为 所 需 的 值 , 并 将 参考 速度 发 送 到 库 卡 
机 器 人 控制 器 (KRC) : 它 使 用 低级 库 卡 控制 跟踪 速度 曲线 。 其 用 法 为 : kctmovejoint(vet)， 
其 中 参数 vet 为 关节 角速度 向 量 。 


运行 结果 : 


>> kctmovejoint([2 2 2 2 2 2]) 
Error using icinterface/fwrite (line 191) 
OBJ must be connected to the hardware with FOPEN. 
Error in kctmovejoint (line 119) 
fwrite(t, robotspeed); 


由 于 没有 连接 到 硬件 ,不 能 得 到 最 终 的 测试 结果 。 

ketmovexyz() 

函数 kctmovexyz() 的 功能 是 设置 未 端 执 行 器 的 线性 速度 和 角速度 ,并 将 参考 速度 发 送 到 
库 卡 机 器 人 控制 器 (KRC): 它 使 用 低级 库 卡 控制 跟随 速度 分 布 。 其 用 法 为 : kctmovexyz(vet)， 
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其 中 ,参数 vet 为 末端 执行 器 的 线性 向 量 (vet(1:3)) 和 角速度 (vet(4:6))。 
运行 结果 : 
>> kctmovexyz([10 10 10 2 2 2]) 
Error using icinterface/fwrite (line 191) 
OBJ must be connected to the hardware with FOPEN. 


Error in kctmovexyz (line 128) 
fwrite(t, robotspeed); 


由 于 没有 连接 到 硬件 ,不 能 得 到 最 终 的 测试 结果 。 

kctpathjoint( ) 

函数 kctpathjoint() 的 功能 是 沿 着 给 定 轨迹 移动 机 器 人 的 末端 执行 器 ,给 定 轨迹 为 关节 
空间 中 的 一 系列 指定 分 配 的 位 姿 。 其 用 法 为 kctpathjoint C points. vp. plo . 其 中 参数 
points 为 包含 末端 效应 器 位 姿 的 nX6 矩阵 ,vp 为 所 支持 的 最 大 速度 的 百分比 (如 果 未 指 
定 , 则 设置 默认 值 (vp = 2026) plot 为 标志 变量 : 若 为 1, 则 显示 轨迹 , 若 为 0, 则 不 显示 轨 
迹 。 返 回 值 robotinfo 为 结构 体 , 其 包含 3 个 参数 : realtraj 为 末端 执行 器 沿 实际 轨迹 的 姿态 
序列 ,idealtraj 为 末端 效应 器 沿 期 望 轨迹 的 姿态 序列 ,warn 为 二 进 制 变量 ,如 果 warn = 1. 
则 发 生 问题 。 

运行 结果 : 


>> kctpathjoint([90 20 45 30 45 30],30,1) 
Error using icinterface/fwrite (line 191) 
OBJ must be connected to the hardware with FOPEN. 
Error in kctreadstate (line 51) 
fwrite(t, '<ask/>'); 
Error in kctpathjoint (line 53) 
CurrentState = kctreadstate(); 


由 于 没有 连接 到 硬件 ,不 能 得 到 最 终 的 测试 结果 。 

kctpathxyz() 

函数 kctpathxyz() 的 功能 是 沿 着 给 定 的 轨迹 移动 机 器 人 的 末端 执行 器 ,轨迹 通过 一 系 
列 指定 分 配 的 姿势 Lz,y,z'roll,pitch,yaw] 确 定 。 其 用 法 为 : kctpathxyz(points,vp, plot)， 
其 中 参数 points 为 包含 末端 效应 器 姿态 的 nX6 和 矩阵 ,vp 为 所 支持 的 最 大 速度 的 百分比 (如 
果 未 指定 , 则 设置 默认 值 (vp 二 20%)) plot 为 标志 变量 : 车 为 1, 则 显示 轨迹 ,车 为 0, 则 不 
显示 。 返 回 值 robotinfo 为 结构 体 ,其 包含 3 个 参数 : realtraj 为 末端 执行 器 沿 实 际 轨迹 的 姿 
态 序 列 ,idealtraj 为 末端 效应 器 沿 期 望 轨迹 的 姿态 序列 ,warn 为 二 进 制 变量 ,如 果 warn 一 1， 
则 发 生 问 题 。 

运行 结果 : 


>> kctpathxyz([400 500 400 45 45 60],30,1) 
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Error using icinterface/fwrite (line 191) 
OBJ must be connected to the hardware with FOPEN. 
Error in kctreadstate (line 51) 
fwrite(t, '<ask/>'); 
Error in kctpathxyz (line 60) 
CurrentState = kctreadstate(); 


由 于 没有 连接 到 硬件 ,不 能 得 到 最 终 的 测试 结果 。 

kctstop( ) 

函数 ketstop() 的 功能 是 在 当前 位 置 停止 机 器 人 , 即 发 送 零 速度 设 定 值 以 停止 机 器 人 。 
其 用 法 为 ketstopO 。 

函数 kctstop() 的 程序 如 下 所 示 : 


function kctstop() 
global kctipvar; 
t = kctipvar; 
global kcttcpiptype; 
global kcttom; 


kcttom; 
if (kcttom == 0) || (kcttom == 1) 
A1 - ['"0.000"']; 
A2 - ['"0.000"']; 
A3 - ['"0.000"']; 
a4 = ['"0.000"']; 
A5 = ['"0.000"']; 


A6 - ['"0.000"']; 
robotspeed- [ '< AKorr A1 = ',A1, ' A2 = ', A2, ' A3 = ', A3, M = ', M, ' A5 = ', A5, ' A6 = ', A6, 
人 
else 
= [""0.000"']; 
= "O000 
| Bw’ G, a 
['"0.000"']; 
= [""0.000"']; 
- ["0.000"']; 
robotspeed- [ '< RKorr X= ',X,'Y- wz Z- ',Z, A- ', A, B- ',B,'C- ',C,' /»']; 
end 


" 


Q U » NM «x 


if kcttcpiptype == 'ICT' 
fwrite(t, robotspeed); 
elseif kcttcpiptype == 'MEX' 
kctsenddatamex(robotspeed); 
end 


分 析 以 上 程序 可 知 ,其 将 机 器 人 关节 角速度 或 末端 执行 器 的 速度 均 变 为 0, 以 达到 使 机 
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器 人 停止 运动 的 目的 。 
运行 结果 : 
2» kctstop() 
Error using icinterface/fwrite (line 191) 
OBJ must be connected to the hardware with FOPEN. 
Error in kctstop (line 57) 
fwrite(t, robotspeed); 


由 于 没有 连接 到 硬件 ,不 能 得 到 最 终 的 测试 结果 。 

kcthome() 

函数 kcthome( ) 的 功能 是 驱动 机 器 人 回 到 初始 位 置 , 初 始 位 置 在 函数 kctrsiclient. src 
中 定义 。 其 用 法 为 kcthome() 。 

运行 结果 : 


kctdrivegui( ) 

函数 ketdrivegui ) 的 功能 是 显示 机 器 人 运动 的 图 形 界面 ,其 用 法 是 kctdrivegui( )。 

运行 结果 : 

弹出 以 下 两 个 用 户 界 面 。 如 图 1-59 所 示 , 通 过 这 两 个 图 形 用 户 界 面 ,用 户 可 以 自由 地 
设置 关节 角 和 运动 轨迹 并 观察 机 器 人 的 运动 。 


关节 角 控 制图 形 界面 : : 
w— - x KUKA 机 器 人 运动 界面 


图 1-59 ”函数 kctdrivegui() 生 成 的 机 器 人 运动 图 形 界面 


kctcheckarg() 
函数 kctcheckarg() 的 功能 是 检查 KCT 函数 的 参数 ,并 返回 KCT 例 程 所 需 使 用 的 值 。 
从 严格 意义 上 来 说 ,函数 kctcheckarg() 应 该 算是 一 个 诊断 函数 。 其 用 法 为 : kctoption 一 
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kctcheckarg( varargin), 其 中 参数 'Plot', 布 尔 变量 (1/0) 可 以 启用 /禁用 绘图 ; 
'FrameDimension' ,float 变量 可 以 设置 末端 效应 器 框架 尺寸 ; 'FrameStep' ,float 变量 可 以 
设置 末端 效应 器 框架 步 长 ; 'FrameColor', 可 以 确定 末端 效应 器 框架 的 颜色 ;'TrajColor'， 
可 以 确定 机 器 人 轨迹 的 颜色 ;'ShowRobot' ,布尔 变量 (1/0) 可 以 启用 /禁用 机 器 人 的 绘图 ; 
'View'" ,可 以 设 定 视点 规格 。 返 回 值 为 包含 以 上 项 目 相对 应 值 的 结构 体 。 

5) 图 像 函数 

图 像 函 数 主要 用 于 绘画 和 显示 机 器 人 的 运动 ,共有 4 个 函数 ,如 下 所 示 : 

kctdisprobot( ) 

函数 kctdisprobot() 的 功能 是 在 用 户 定 义 的 位 置 显示 机 器 人 的 3D 模型 。 其 还 会 显示 
出 基本 参考 系 < x0,y0,z0 > 和 末端 效应 器 参考 系 < x6,y6,z6 >。 其 用 法 为 : kctdisprobot 
CangleDH.h. fig) ,其 中 参数 angleDH 为 所 需 关 节 角 向 量 ,h_fig 为 图 的 处 理 程序 数 , 如 果 未 
指定 , 则 设置 默认 值 。 返 回 值 h_fig 为 图 处 理 程序 数 。 

运行 结果 : 


>> kctdrivegui() 
>> kctdisprobot([90 20 45 30 45 30]) 
ans = 

1 


Ait AN Ac Js SEG — ES TUR PR. de B] 1-60 所 示 , 显 示 出 当前 机 器 人 的 3D 模型 的 
底座 坐标 系 和 末端 执行 器 参考 系 。 


X 
图 1-60 机 器 人 底座 坐标 系 及 未 端 执行 器 参考 系 


kctdisptraj( ) 

函数 kctdisptraj() 的 功能 是 显示 末端 执行 器 的 3D 轨迹 和 机 器 人 的 初始 和 最 终 姿态 。 
其 用 法 为 : kctdisptraj(state,option), 其 中 参数 state 为 nX6 的 关节 角 矢 量 和 矩阵 ,option 为 
指定 其 他 属性 的 参数 对 。 例 如 , 'Plot' ,布尔 变量 (1/0) 可 以 启用 /禁用 绘图 ; 


110 4| 机 器 人 仿真 与 编程 技术 


'FrameDimension' ,float 变量 可 以 设置 末端 效应 器 框架 尺寸 ; 'FrameStep' ,float 变量 可 以 
设置 末端 效应 器 框架 步 长 ; 'FrameColor', 可 以 确定 末端 效应 器 框架 的 颜色 ; ' TrajColor'. 
可 以 确定 机 器 人 轨迹 的 颜色 ;'ShowRobot' ,布尔 变量 (1/0) 可 以 启用 /禁用 机 器 人 的 绘图 ; 
"View', 可 以 设 定 视点 规格 。 


运行 结果 : 
>> kctdisptraj([90 20 45 30 45 30;90 10 45 30 45 30]) 


输入 函数 后 弹出 一 个 图 形 用 户 界面 。 如 图 1-61 所 示 , 它 显示 末端 执行 器 的 3D 轨迹 和 
机 器 人 的 初始 位 姿 和 最 终 姿态 。 


图 1-61 机 器 人 的 初始 位 次 和 最 终 位 姿 


kctdispdyn( ) 

函数 kctdispdyn() 的 功能 是 显示 参考 和 实际 关节 角度 随时 间 变 化 的 图 形 。 其 用 法 为 : 
kctdispdyn(state,linecolor) ,其 中 参数 state Jg n 6 的 关节 角 矢 量 矩 阵 ,linecolor 为 行 的 颜色 。 

运行 结果 : 


>> kctdispdyn([90 20 45 30 45 30;90 10 45 30 45 30]) 


输入 函数 后 弹出 一 个 图 形 用 户 界面 。 如 图 1-62 所 示 , 它 显示 参考 和 实际 关节 角度 的 时 
间 曲 线 。 

kctanimtraj © 

函数 ketanimtraj C) 的 功能 是 显示 机 器 人 沿 给 定 轨迹 的 3D 运动 动画 。 其 用 法 : 
kctanimtraj(state.option) ,其 中 参数 state Jy n2 6 的 关节 角 矢量 和 矩阵 ,option 为 指定 其 他 
属性 的 参数 对 。 例 如 ,'Plot' ,布尔 变 量 (1/0) 可 以 启用 /禁用 绘图 ; 'FrameDimension' ,float 
变量 可 以 设置 末端 效应 器 框架 尺寸 ; 'FrameStep' ,float 变量 可 以 设置 末端 效应 器 框架 步 
K; 'FrameColor', 可 以 确定 末端 效应 器 框架 的 颜色 ;'TrajColor', 可 以 确定 机 器 人 轨迹 的 颜 
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图 1-62 函数 kctdispdyn() 生 成 的 关节 角 - 时 间 曲 线 
色 ; 'ShowRobot', 布 尔 变量 (1/0) 可 以 启用 /禁用 机 器 人 的 绘图 : "View' ,可 以 设 定 视点 规格 。 
运行 结果 : 
>> kctanimtraj([90 20 45 30 45 30;90 10 45 30 45 30;80 10 45 30 45 30], 2, 'Plot',1) 
kctanimtraj finished... 
输入 函数 后 弹出 一 个 图 形 用 户 界面 。 如 图 1-63 所 示 , 它 显示 机 器 人 沿 给 定 轨迹 的 3D 
运动 动 图 。 


1-63 ”函数 kctanimtraj() 生 成 的 机 器 人 轨迹 动 图 


6) 齐 次 变换 
齐 次 变换 主要 用 于 坐标 的 变换 ,共有 6 个 函数 。 
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kctrotox( ) 
函数 ketrotox() 的 功能 是 计算 围绕 X 轴 旋 转 的 齐 次 旋转 矩阵 ,其 中 旋转 角度 以 弧度 表 
示 。 其 用 法 为 : H = kctrotox (alpha) .其 中 参数 alpha 为 绕 zx 轴 旋 转 的 旋转 角 ( 以 角度 表 
示 ) ,返回 值 为 一 个 4X4 的 齐 次 旋转 矩阵 。 
空间 中 某 一 点 A, 不 妨 设 其 坐标 为 (Xa Ya. Za) ,该 点 绕 X 轴 旋 转 0 角 后 到 达 Al 点 ,不 
妨 设 A1 的 坐标 为 (Xm1 Ya Za .Al 点 和 A 点 的 坐标 关系 为 
Xa = Xa 
Ya = Ya cos(0) — Za sin(0) 
Za = Ya sin(0) + Zacos(0) 


Xa 1 0 0 Xa 
a| = | cos(0) 一 mo) H 
Za 0 sin(0) cos(0)J LZa 


Hä kctrotox() 的 程序 如 下 所 示 : 


用 和 矩阵 表示 为 


function H= kctrotox(alpha) 
alpha = alpha * pi/180; 
H = [1, 0, 0, 0; 
0, cos(alpha), - sin(alpha), 0; 
0, sin(alpha), cos(alpha), 0; 
0, 0, 0, MI 


由 以 上 程序 可 知 ,该 函数 先 将 旋转 角 的 角度 转化 为 弧度 ,再 求 取 齐 次 旋转 矩阵 。 
运行 结果 : 


>> H = kctrotox(180) 


H= 
1.0000 0 0 0 
0 —1.0000 - 0.0000 0 
0 0.0000  — 1.0000 0 
0 0 0 1.0000 
kctrotoy ©) 


函数 kctrotoy() 的 功能 是 计算 围绕 Y 轴 旋 转 的 齐 次 旋转 矩阵 ,其 中 旋转 角度 以 弧度 表 
示 。 其 用 法 为 : H = kctrotoy(alpha) ,其 中 参数 alpha 29 £& Y 轴 旋 转 的 旋转 角 (以 角度 表 
示 ) ,返回 值 为 一 个 4X4 的 齐 次 旋转 矩阵 。 
空间 中 某 一 点 A ,不 妨 设 其 坐标 为 (Xs .Ya .2Za) «Vos e Y 轴 旋 转 0 角 后 到 达 Al 点 ,不 
妨 设 A1 WERI Xa Y a Za .Al 点 和 A 点 的 坐标 关系 为 : 
Xa = Xacos(0) + Za sin(O) 
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Ya = YA 
Za =— Xa sin(0) + Zacos(0) 


Xa cos(0) 0 sin(0)] [X4 
a| = | 0 1 0 | 四 
Zal —sin(0) 0 cos(0)J LZa 


函数 kctrotoy() 的 程序 如 下 所 示 : 


用 矩阵 表示 为 : 


function H = kctrotoy(alpha) 
alpha = alpha * pi/180; 


H = [cos(alpha), 0, sin(alpha), 0; 
LP 9, 0; 

— sin(alpha), 0, cos(alpha), 0; 
0, 0, 0, 1]; 


由 以 上 程序 可 知 ,该 函数 先 将 旋转 角 的 角度 转化 为 弧度 ,再 求 取 绕 YY 轴 旋 转 后 的 齐 次 


旋转 矩阵 。 


m. 
m. 


运行 结果 : 


> H = kctrotoy(90) 


H = 
0.0000 0 1.0000 0 

0 1.0000 0 0 

— 1.0000 0 0.0000 0 

0 0 0 1.0000 


kctrotoz() 

函数 kctrotoz() 的 功能 是 计算 围绕 Z 轴 旋 转 的 齐 次 旋转 矩阵 ,其 中 旋转 角度 以 弧度 表 
其 用 法 为 : H = kctrotoz(alpha) ,其 中 参数 alpha 为 绕 Z 轴 旋 转 的 旋转 角 ( 以 角度 表 
返回 值 为 一 个 4X4 的 齐 次 旋转 矩阵 。 

空间 中 某 一 点 A ,不 妨 设 其 坐标 为 (Xa YA Za) ,该 点 绕 Z 轴 旋 转 0 角 后 到 达 Al 点 ,不 


妨 设 A1 的 坐标 为 (X41 Ya ZaD ,Al 点 和 人 A 点 的 坐标 关系 为 : 


Xa = Xa cos(0) 一 YA sin(O) 
Ya = Xa sin(0) 十 Yacos(O) 
Zu = Za 


Xa cos(0) —sin(®) 0] FXA 
je = [o cos(0) | H 
Za 0 0 Tl 67, 


用 和 矩阵 表示 为 : 
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函数 kctrotoz() 的 程序 如 下 所 示 : 


function H = kctrotoz(alpha) 
alpha = alpha * pi/180; 
H = [cos(alpha), -sin(alpha), 0, 0; 
sin(alpha), cos(alpha), 0, 0; 
0, Dk. Eee 
0, 0, 0,1]; 


由 以 上 程序 可 知 , 该 函数 先 将 旋转 角 的 角度 转化 为 弧度 ,再 利用 公式 6-3-1 求 取 绕 Z 轴 
旋转 后 的 齐 次 旋转 矩阵 。 
运行 结果 : 


>> H = kctrotoz(45) 


H= 
0.7071 -0.7071 0 0 
0.7071 0.7071 0 0 

0 0 1.0000 0 
0 0 0 1.0000 
kcttran() 


函数 kcttran() 的 功能 是 计算 平移 后 的 齐 次 矩阵 ,其 中 平移 长 度 以 毫米 为 单位 。 其 用 法 
为 : H = kcttran(tran_vet) ,其 中 参数 tran. vet 为 平移 向 量 [dx.dy.dz], 返 回 值 H 为 平移 
后 的 齐 次 矩阵 。 

空间 中 一 点 A, 不 妨 设 其 坐标 为 (XA,YaA,ZaA) ,该 点 经 过 平移 后 到 达 Al 点 ,不 妨 设 Al 
的 坐标 为 C(XAa Ya Za EE RP I6] ib [dc dy dz ] W) AT 点 和 A 点 的 坐标 关系 为 


Xa = Xa + dr 
Ya = Y4 + dy 
Zm = Za + dz 


函数 kcttran() 的 程序 如 下 所 示 : 


function H = kcttran(tran vet) 

H-2[ 1,0, O0, tran vet(1,1); 
0,1, 0, tran vet(1,2); 
0, 0, 1, tran vet(1,3); 
0,0, 0,1 ]; 


分 析 以 上 程序 可 知 ,该 函数 保持 AYZ 轴 方 向 不 变 , 将 X,Y,Z 轴 坐 标 与 相应 的 平移 
量 相 加 得 到 平移 后 的 齐 次 答 阵 。 
运行 结果 : 
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kctchframe() 

函数 kctchframe() 的 功能 是 更 改 机 器 人 的 参考 系 ,工具 箱 默 认 的 基准 参考 系 为 < x0， 
y0,z0>, 用 户 可 以 新 增 自 定义 的 参考 系 < xw,yw,zw >, 参 考 系 储存 在 文件 kctrobotframe 
.mat 中。 其 用 法 为 : kctchframe(HOw) ,其 中 参数 How 为 基本 参考 系 < x0,y0,z0 > 和 新 的 
参考 系 < xw,yw,zw > 之 间 刚 性 运动 的 齐 次 矩阵 。 

函数 kctchframe() 的 程序 如 下 : 


运行 结果 : 


kctgetframe( ) 
函数 kctgetframe( ) 的 功能 是 返回 当前 的 笛 卡 儿 坐标 系 , 所 有 运动 均 是 相对 于 该 坐标 系 
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进行 的 。 其 用 法 为 : Hfr = kctgetframe(), 其 中 返回 值 Hfr 为 当前 的 笛 卡 儿 坐 标 系 和 基本 
参考 坐标 系 之 间 刚 性 运动 的 齐 次 矩阵 。 
函数 kctgetframe( ) 的 程序 如 下 所 示 : 


function currentframe = kctgetframe 

global kctrotfr; 

global kctptfr; 

currentframe = [kctrotfr,kctptfr';0 0 0 1]; 


运行 结果 : 


>> Hfr = kctgetframe() 

Hfr = 

0 0 0 1 

>> HOw= [10 0 100010 100;0 0 1 100;00 0 1] 


HOw = 
Hu 0 0 100 
0 1 0 100 
0 0 1 100 


0 0 0 1 
>> kctchframe(HOw) 
>> Hfr = kctgetframe() 
Hfr 


ooonciu 
ooro 
oroo 

m 

© 

o 


7) 演示 函数 

演示 函数 共有 3 个 ,如 下 所 示 : 

kctdemo( ) 

函数 ketdemo() 的 功能 是 KUKA 控制 工具 箱 的 演示 , 即 进行 一 些 简单 和 基本 的 操作 演 
示 , 以 增进 用 户 对 本 工具 箱 的 理解 和 运用 。 其 用 法 为 kctdemo()。 

函数 kctdemo() 的 程序 如 下 所 示 : 


fprintf(' KUKA Control Toolbox Demo. Vn Press any key to continue... \n') 
pause 
kctsel = input('If you want to connect the robot press 1, any key if it is already connect: '); 
if kctsel == 1 
kctmodel = input(' Insert a string with your KUKA robot model: '); 
kctinit(kctmodel) 
addr = input(' Insert a string with your KUKA controller''s IP address: '); 
t = kctclient(addr, 0.15); 
end 
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disp(' Press any key to move the robot. ') 
pause 
disp(' Robot moving...') 
pause(0.1); 
[robotstate,warn] = kctsetjoint([0 0 45 0 —45 0],50); 
disp(' Press any key to display the dynamics of the robot:') 
pause 
kctdispdyn(robotstate); 
disp(' Press any key to move the robot along a square path: ') 
pause 
disp(' Home positioning...') 
kcthone() 
disp('...") 
P-[450 225100 090 0; 
450 225 5501060 - 10; 
450 -225 550 090 0; 
450 -225 100 090 20; 
450 225100 090 0]; 
kctpathxyz(P, 30,1) ; 
disp(' Press any key to use the KCT GUI for robot motion control:') 
pause 
kctdrivegui 
disp(' The demo is finished. ') 


由 以 上 程序 可 知 ,其 演示 的 流程 为 : 选择 连接 机 器 人 ,输入 库 卡 机 器 人 模型 ,输入 库 卡 
机 器 人 控制 端的 IP 地 址 ,移动 机 器 人 ,演示 机 器 人 的 运动 , 沿 着 正方 形 路 径 移 动机 器 人 , 邻 
机 器 人 回 到 初始 位 置 , 调 用 函数 加 载 用 户 图 形 界面 。 

运行 结果 : 


>> kctdemo( ) 
KUKA Control Toolbox Demo. 
Press any key to continue... 
If you want to connect the robot press 1, any key if it is already connect:1 
Insert a string with your KUKA robot model: 'KR3' 
Insert a string with your KUKA controller's IP address: '192. 168.1. 0' 
KCTCLIENT: connect the robot with MATLAB 
Start the kctserver.exe on the KUKA robot controller and press any key to continue... 
Connection on port failed 
Frame position and orientation 
kctptfr = 
0 0 0 
TCPIP Object : TCPIP - 192.168.1.0 
Communication Settings 
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RemotePort: 2999 
RemoteHost: 192.168.1.0 
Terminator: LE" 
NetworkRole: client 
Communication State 
Status: closed 
RecordStatus: off 
Read/Write State 
TransferStatus: idle 
BytesAvailable: 0 
ValuesReceived: 0 
ValuesSent: 0 
kctrotfr - 
pl 0 0 
0 1 0 
0 0 1 
Sample time for communication 
Ta 
0.1500 
Press any key to move the robot. 
Robot moving... 


Error using icinterface/fwrite (line 191) 
OBJ must be connected to the hardware with FOPEN. 
Error in kctreadstate (line 51) 
fwrite(t, '& ask/»') ; 
Error in kctsetjoint (line 111) 
robotstate - kctreadstate(); 
Error in kctdemo (line 41) 
[robotstate,warn] = kctsetjoint([0 0 45 0 —45 0],50); 


由 于 该 演示 过 程 涉及 硬件 连接 , 故 不 进行 相关 的 测试 。 

ketdemohaptik () 

函数 kctdemohaptik() 的 功能 是 使 用 HaptiK Library 演示 KUKA 控制 工具 箱 ,其 允许 通过 
触觉 界面 控制 机 器 人 操纵 器 (需要 Haptik Library 1.0) 。 其 用 法 为 kctdemohaptik()。 

由 于 该 演示 过 程 涉 及 硬件 连接 , 故 不 进行 相关 的 测试 。 

ketdemovision () 

函数 kctdemovision O 功能 是 在 视觉 伺服 中 演示 KUKA 控制 工具 箱 , 其 用 法 为 
kctdemovisionO 。 

由 于 该 演示 过 程 涉及 硬件 连接 , 故 不 进行 相关 的 测试 。 

5. 绘画 一 个 圆 

以 上 为 单个 函数 的 测试 ,下 面 通过 组 合 一 些 函 数 实现 某 一 特定 功能 : 利用 机 器 人 末端 
执行 器 绘画 一 个 圆 。 其 程序 如 下 所 示 : 
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>> kctsetbound([ - 800 800 — 800 800 0 800; - 90 180 -90 180 -90 180]) 
> k= [0:pi/50:2 x pi]; 
>> x 7 600 x ones(1, length(k)) ; 
>> y7150* cos(k); 
>> 27150 * sin(k) + 400; 
>> p7 [x', y', z', repnat([0 90 0], length(k),1)]; 
>> kctpathxyz(p,20,1); 
Error using icinterface/fwrite (line 191) 
OBJ must be connected to the hardware with FOPEN. 
Error in kctreadstate (line 51) 
fwrite(t, '< ask/»') ; 
Error in kctpathxyz (line 60) 
CurrentState = kctreadstate(); 


由 于 没有 连接 到 硬件 ,单独 运行 以 上 代码 并 没有 成 功 地 生成 动画 及 图 形 。 如 图 1-64 所 
示 , 它 是 程序 运行 成 功 时 应 该 弹出 的 用 户 图 形 界面 。 


20 
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图 1-64 利用 机 器 人 未 端 执行 器 绘画 一 个 圆 


1.7.2 其 他 机 器 人 工具 箱 


基于 MATLAB 的 机 器 人 工具 箱 还 有 不 少 ,它们 或 多 或 少 都 具有 一 些 突出 的 优点 和 不 
足 , 下 面 将 介绍 几 个 MATLAB 机 器 人 工具 箱 ,与 KCT 形成 对 比 。 

1. KUKA-KRL-Tbx 

KUKA-KRL-Tbx 是 由 维 斯 马 大 学 的 计算 工程 和 自动 化 研究 小 组 开发 的 。 它 实现 了 具 
有 SCE 的 工业 机 器 人 的 控制 ,如 MATLAB 和 Scilab。 该 工具 箱 通过 基于 KUKA 的 KR3 
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机 器 人 原型 开发 进行 测试 和 验证 。 由 于 对 机 器 人 技术 的 研究 正在 不 断 地 发 展 ,机 器 人 的 应 
用 领域 也 在 不 断 地 增加 ,使 得 对 于 机 器 人 的 要 求 非常 高 ,编程 简单 和 集成 不 同 的 外 部 硬件 
(如 传感器 ,操作 器 ) 显 得 特别 重要 。 在 这 种 情况 下 ,人 们 期 望 提供 从 早期 设计 到 操作 阶段 的 
连续 平稳 软件 开发 环境 。 

在 工程 和 科学 领域 中 ,控制 模型 设计 通常 的 特征 在 于 使 用 科学 和 技术 计算 环境 (Super 
Computing Environment. SCE) ,如 MATLAB 和 已 经 存在 的 其 他 自由 SCE, 如 Scilab 或 
Octave。KUKA-KRL-Tbx 工具 箱 弥补 了 机 器 人 制造 商 特定 的 编程 语言 (例如 KUKA 机 器 
人 语言 (KRL)) 和 SCE 之 间 的 差距 。 如 图 1-65 所 示 , 显 示 了 通过 添加 包含 MATLAB 或 
Scilab 的 PC 来 扩展 KUKA 环境 的 示例 。 


KUKA KR3 机 器 人 


包含 MATLAB 或 Scilab 


并 安装 工具 箱 
的 计算 机 


具有 KRL 语 言 解释 器 的 
库 卡 KR3 控 制 器 


ET M 
eta. M^ 
"trim 
图 1-65 KRL 5j KUKA 控制 器 .KUKA 机 器 人 通信 的 例子 


由 图 1-65 可 知 ,KUKA 控制 器 通过 串 行 接口 与 PC 相连 。 随 后 ,在 KRL 中 实现 了 与 解 
释 器 的 双向 通信 。 解 释 器 在 KUKA 控制 器 上 运行 ,负责 识别 和 执行 由 PC 发 送 的 命令 。 控 
制程 序 是 使 用 MATLAB 或 Scilab 在 KUKA-KRL-Toolbox 的 基础 上 开发 ,在 PC 上 运行 。 

与 其 他 的 机 器 人 工具 箱 相 比较 ,KUKA-KRL-Toolbox 具有 以 下 优点 : 

。 提供 从 早期 设计 到 操作 阶段 的 连续 均匀 软件 环境 

。 可 以 轻松 集成 附加 硬件 ; 

。 使 用 仿真 的 控制 策略 的 综合 测试 ; 

* KRL 通过 工作 区 监控 提供 的 安全 具有 永久 性 , 它 会 检查 每 个 机 器 人 轴 的 最 终 位 置 

开关 等 。 

它 实现 了 从 设计 到 操作 使 用 MATLAB 或 Scilab-Toolboxes 的 可 能 性 。 

与 KCT 对 比 可 以 发 现 : KUKA-KRL-Toolbox 缺少 图 形 用 户 界 面 ,没有 实时 跟踪 功 
能 。 同 时 它 还 具有 一 些 自身 的 不 足 : 其 工具 箱 中 的 MATLAB 命令 与 KRL 函数 是 一 对 一 
通信 ,阻碍 了 用 户 设 计 先 进 的 控制 应 用 程序 ; 串 行 接口 不 允许 高 速 传输 等 。 

2. Machine Vision Toolbox(MVT) 

Machine Vision Toolbox(MVT) 是 一 个 用 于 机 器 视觉 的 开源 工具 箱 , 工 具 箱 中 表现 出 
作者 对 光度 、 摄 影 测量 和 比 色 法 领域 的 兴趣 。MVT 包括 超过 60 个 功能 ,包括 图 像 文 件 读 
取 和 写 入 、 获 取 、 显 示 、 过 滤 、 斑 点 、 点 和 线 特征 提取 数学 形态 学 、 单 应 性 、 视 觉 雅 可 比 、 相 机 
校准 和 颜色 空间 转换 。MVT 可 用 于 机 器 视觉 的 研究 ,但 也 具有 足够 的 通用 性 ,可 用 于 实时 
工作 甚至 控制 。MVT 结合 MATLAB 和 模型 工作 站 计算 机 ,是 一 种 用 于 机 器 视觉 算法 调 
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查 的 有 用 和 方便 的 环境 。 对 于 适度 的 图 像 大 小 ,处 理 速率 可 以 是 足够 实时 的 以 允许 闭环 控 
制 。 诸 如 动态 窗口 (未 提供 ?的 注意 力 焦点 方法 可 以 用 于 增加 处 理 速率 。 使 用 来 自 火线 或 网 
络 摄像 机 (提供 支持 ) 和 输出 到 机 器 人 (未 提供 ) 的 输入 ,可 以 完全 在 MATLAB 中 实现 可 视 
伺服 系统 。 

图 像 通常 被 视 为 表示 强度 或 可 能 范围 的 标量 值 的 矩形 阵列 。 该 矩阵 是 MATLAB 的 自 
然 数 据 类 型 ,这 使 得 图 像 的 操作 在 MATLAB 语言 中 的 算术 语句 中 很 容易 表示 。 许 多 图 像 
操作 . np fé .滤波 和 统计 可 以 用 现 有 的 MATLAB 函数 实现 。MVT 使 用 实现 函数 和 类 的 
M 文件 以 及 用 于 某 些 计算 密集 型 操作 的 MEX 文件 来 扩展 此 核心 功能 。 可 以 使 用 MEX X 
件 与 图 像 采 集 硬 件 接口 ,从 简单 的 帧 加 速 器 到 机 器 人 。MATLAB 矢量 化 已 被 尽 可 能 地 用 
于 提高 效率 ,然而 ,一 些 算法 不 适 于 矢量 化 。 一 些 特别 是 计算 密集 型 功能 被 提供 为 MEX 文 
件 ,并 且 可 能 需要 为 特定 平台 编译 。MVT 将 图 像 通常 视 为 双 精 度数 字 的 数组 。 

虽然 有 很 多 共同 的 功能 ,但 MVT 不 是 Mathworks 公司 的 图 像 处 理工 具 箱 (IPT) 的 克 
隆 。 这 个 工具 箱 的 第 三 个 版 本 已 经 扩大 到 包括 代表 不 同类 型 的 相机 (透视 、 鱼 眼 、 反 射 和 球 
面 ) 姿态 估计 、 视 觉 雅 可 比 和 高 级 分 割 技术 ,如 MSER 和 基于 图 表 的 类 。 工 具 箱 还 包括 用 
于 臂 式 、 移 动 和 飞行 机 器 人 的 PBVS 和 IBVS 视觉 伺服 系统 的 Simulink 模型 。 

MVT 具有 以 下 优点 : 代码 相当 成 熟 ,并 为 同一 算法 的 其 他 实现 提供 了 比较 点 ; 这 些 例 
程 通常 以 直接 的 方式 编写 ,这 允许 容易 地 理解 ,或 许 以 牺牲 计算 效率 为 代价 进行 编写 ,如 果 
想 提 高 计算 效率 ,可 以 重 写 函 数 来 实现 ,可 以 使 用 MATLAB 编译 器 编译 M 文件 或 创建 
MEX 版 本 。 

3. Robotics System Toolbox 

Robotics System Toolbox 为 开发 自动 化 移动 机 器 人 应 用 程序 提供 算法 和 硬件 连接 。 
工具 箱 算法 包括 差分 驱动 机 器 人 的 地 图 表示 、 路 径 规划 和 路 径 跟踪 ,可 以 设计 和 电机 控制 、 
计算 机 视觉 和 状态 机 在 MATLAB 应 用 程序 或 Simulink 的 原型 ,并 将 它们 与 机 器 人 系统 工 
具 箱 核心 算法 集成 。 系 统 工 具 箱 提供 了 MATLAB 和 Simulink 之 间 的 接口 和 机 器 人 操作 
系统 (ROS) ,使 用 户 能 够 测试 和 验证 启用 ROS 的 机 器 人 和 机 器 人 仿真 器 (如 Gazebo) 上 的 
应 用 程序 。 它 支持 C++ 代码 生成 ,使 用 户 能 够 从 Simulink 模型 生成 ROS 节点 并 将 其 部 署 
到 ROS 网 络 。 机 器 人 系统 工具 箱包 括 示例 ,显示 如 何 使 用 Gazebo 中 的 虚拟 机 器 人 和 启用 
ROS 的 机 器 人 o 

Robotics System Toolbox 的 主要 功能 有 : 路 径 规 划 ,路 径 跟踪 和 地 图 表示 算法 ; 用 于 
在 不 同 旋转 和 平移 表示 之 间 转 换 的 函数 ; 与 启用 ROS 的 机 器 人 进行 双向 通信 ; 接口 到 
Gazebo 和 其 他 启用 ROS 的 仿真 器 ; 从 rosbag 日 志文 件 导 入 数据 ; 从 Simulink 中 ROS 节 
点 代 车 型 (使 用 Simulink 编码 器 ) 。 

对 于 坐标 变换 ,在 机 器 人 应 用 中 ,可 以 使 用 许多 不 同 的 坐标 系 来 定义 机 器 人 、 传 感 器 和 
其 他 物体 位 于 何 处 。 通 常 ,3D 空间 中 的 对 象 的 位 置 由 其 位 置 和 取向 来 定义 。 机 器 人 系统 工 
具 箱 支持 机 器 人 中 常用 的 坐标 表示 :并 允许 用 户 在 它们 之 间 进 行 转换 。 当 将 这 些 坐 标 表 示 
应 用 于 3D 点 时 ,也 可 以 在 坐标 系 之 间 进 行 转换 。 轴 角 、 欧 拉 角 、 齐 次 变换 和 矩阵、 四 元 数 、 旋 
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转 矩 阵 和 平移 矢量 是 一 些 支持 的 坐标 表示 。 

对 于 互动 数据 探索 ,Robotics System Toolbox 提供 MATLAB 和 Simulink 中 与 机 器 人 
操作 系统 (ROS) 之 间 的 接口 。 使 用 此 接口 ,可 以 连接 到 ROS 网 络 ,使 用 标准 和 专门 的 ROS 
消息 ,与 发 布 者 和 订户 交换 数据 ,调用 和 提供 服务 ,访问 ROS 参数 服务 器 ,导入 rosbag ,并 从 
ROS 访问 转换 树 包 。 可 以 将 来 自任 何 启 用 ROS 的 机 器 人 或 仿真 器 上 的 传感器 的 数据 传送 
到 MATLAB 中 进行 可 视 化 探索、 系统 识别 和 校准 。 还 可 以 向 执行 器 发 送 命 令 以 控制 机 器 
人 ,并 探索 其 功能 和 限制 。 例 如 ,可 以 从 Husky 机 器 人 上 的 ROS 启用 测 距 仪 读 取 数 据 , 以 
验证 其 测量 范围 读数 ; 可 以 直接 从 Baxter 机 器 人 的 相机 读 取 图 像 序列 ,并 执行 相机 校准 ， 
以 查找 视觉 系统 与 计算 机 视觉 系统 工具 箱 的 内 在 和 外 在 参数 ; 可 以 向 TurtleBot 机 器 人 的 
电机 发 送 速 度 命令 ,以 了 解 其 在 不 同 表面 上 的 行为 。 

Robotics System Toolbox 提供 了 包括 差分 驱动 机 器 人 的 路 径 规划 、 路 径 跟 踪 和 地 图 表 
示 的 算法 。 可 以 将 这 些 算 法 与 在 设计 机 器 人 应 用 程序 时 使 用 其 他 MathWorks 工具 开发 的 
电机 控制 .计算 机 视觉 和 状态 机 组 件 集成 。 然 后 ,可 以 在 启用 ROS 的 仿真 环境 (如 Gazebo 或 
V-REP) 中 直接 测试 和 验证 这 些 算法 。 工 具 箱 允 许 通过 ROS 与 仿真 环境 进行 交互 。 例 如 ， 
可 以 自主 直接 从 MATLAB 中 读 取 模型 和 仿真 属性 ,添加 、 构 建 和 删除 对 象 ,施加 力 和 扭矩 
和 测试 机 器 人 。 如 果 无 法 访问 机 器 人 仿真 器 , 则 系统 工具 箱 会 提供 一 个 虚拟 机 映像 ,使 用 该 
虚拟 机 映像 可 以 测试 算法 。 

当 准 备 在 启用 物理 ROS 的 机 器 人 上 测试 算法 时 ,可 以 使 用 通过 仿真 开发 和 测试 的 相同 
算法 。 使 用 最 少 的 代码 更 改 , 可 以 连接 到 物理 机 器 人 ,而 不 是 仿真 器 机 器 人 。 当 连接 到 启用 
ROS 的 机 器 人 时 ,可 以 在 MATLAB 中 交互 式 地 修改 和 调整 算法 ,并 立即 看 到 结果 。 

当 要 验证 和 验证 算法 的 性 能 时 ,可 以 将 ROS 日 志文 件 从 机 器 人 导入 到 MATLAB 中 进 
行 离线 分 析 和 可 视 化 。 工 具 箱 能 够 导入 rosbag 并 检索 有 关 其 内 容 的 信息 。 可 以 在 bag X 
件 中 按时 间 和 主题 阅读 消息 的 子 集 。 可 以 从 包 文件 中 的 一 个 或 多 个 消息 属性 中 将 数据 作为 
时 间 序 列 提取 ,并 将 该 时 间 序 列 用 于 进一步 处 理 。 

Simulink 支持 机 器 人 系统 中 的 ROS System Toolbox 创建 与 ROS 网 络 配合 使 用 的 
Simulink 模型 ,用 于 发 送 和 接收 指定 主题 的 消息 。 可 以 使 用 实时 ROS 网 络 中 的 数据 测试 算 
法 。 一 旦 在 Simulink 中 有 一 个 工作 算法 或 设计 规范 ,就 可 以 为 能 够 在 任何 Ubuntu 系统 上 
运行 的 独立 ROS 节点 生成 C++ 代码 。 可 部 署 的 ROS 节点 是 完全 独立 的 , 它 不 需要 
MATLAB 运行 。 

4. Core Control Toolbox 

Core Control Toolbox 提供 完整 的 源 代 码 ,并 完全 理解 每 个 功能 正在 做 什么 ,用 户 可 以 
查看 和 修改 工具 箱 中 的 任何 功能 ,以 满足 它们 的 特定 需求 。CCT 包含 过 滤 ,图 形 , 数 学 ， 
元 数 , 机 器 人 和 其 他 通用 功能 。 

Core Control Toolbox 具有 控制 和 估计 功能 。 控 制 功能 允许 用 户 按照 想 要 的 方式 工 
f ,用 户 可 以 使 用 许多 不 同 的 技术 来 处 理 控制 设计 ,包括 频 域 , 状 态 空 间 和 特征 结构 分 配 。 
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工具 箱包 含 卡 尔 曼 滤波 器 、 扩 展 卡尔 曼 滤波 器 、 迭 代 扩 展 卡 尔 曼 和 无 限 卡尔 曼 滤 波 器 等 
众多 滤波 器 。 每 个 卡尔 曼 滤波 器 可 以 处 理 和 测量 在 不 同时 间 到 达 的 多 个 测量 源 。 测 量 在 包 
括 指向 测量 功能 的 指针 的 数据 结构 中 ,允许 容易 地 使 用 不 同 的 测量 方法 。 

工具 箱包 括 一 个 递归 的 牛顿 - 欧 拉动 力学 模型 ,可 以 处 理 各 种 机 器 人 ,包括 多 臂 机 器 人 。 
SCARA 用 于 需要 在 平面 中 组 装 的 许多 工业 应 用 中 ,例如 制造 PC 板 , 如 图 1-66 所 示 , 它 为 
利用 CCT 构建 的 一 个 SCARA 机 械 手 。 


图 1-66 利用 CCT 构建 的 SCARA 机 械 手 


Core Control Toolbox 具有 动力 学 和 仿真 功能 , 它 包含 各 种 动力 系统 的 模型 和 仿真 。 
例如 , 托 卡 马克 的 等 离子 体 垂直 定位 模型 ; 永 磁 同步 电机 的 仿真 ; 汽车 悬 架 的 仿真 。 


本 章 小 结 


本 章 主 要 介绍 了 如 何 利 用 MATLAB 机 器 人 工具 箱 解决 机 器 人 学 的 问题 ,包括 机 器 人 
学 的 数学 基础 ,机 器 人 正道 运动 学 ,机 器 人 动力 学 、 运 动 轨迹 和 机 械 辟 关节 控制 等 几 方 面 的 
内 容 。 此 外 ,还 对 工具 箱 的 一 些 函 数 进行 了 详细 的 说 明 。 最 后 ,本 章 对 Kuka 控制 工具 箱 进 
行 了 介绍 ,对 它 包含 的 所 有 函数 进行 了 测试 与 说 明 , 并 简要 介绍 了 其 他 几 个 基于 MATLAB 
的 机 器 人 工具 箱 。 
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EUM MATLAB 机 器 人 
工具 箱 的 应 用 


2.1 基于 学 习 算法 的 机 器 人 触觉 识别 算法 研究 
本 节 将 介绍 机 器 人 触觉 检测 算法 的 原理 、 编 程 与 实现 。 
2.1.1 引言 


在 机 器 人 上 面 实现 触觉 功能 ,使 机 器 人 能 判别 出 被 测 物体 的 各 种 性 质 ( 如 轮廓 、 硬 度 )， 
目前 最 常见 的 方法 是 使 用 专门 的 触觉 传感器 。 触 觉 传感器 的 发 展 始 于 20 世纪 70 年 代 ,至 
今 已 经 研究 出 多 种 多 样 的 功能 各 异形 态 各 异 的 触觉 传感器 ,如 近年 来 很 热门 的 柔性 、 阵 列 触 
觉 传感器 ,然而 这 意味 着 要 使 机 器 人 有 “触觉 ", 就 必须 先 配 置 上 相应 的 触觉 传感器 ,这 对 于 
现 有 的 大 部 分 机 器 人 来 说 门槛 太 高 。 于 是 就 有 了 触觉 算法 的 需求 。 触 觉 算法 一 般 对 机 器 人 
的 硬件 要 求 不 高 ,传统 的 机 器 人 都 具有 测量 所 受 外 力 的 传感器 ,运用 这 个 与 外 界 物体 交互 所 
受 的 外 力 。 本 节 内 容 利用 一 种 通过 和 迭代 学 习 自 适应 的 触觉 算法 ,不 断 调整 更 新 参考 点 的 位 
置 和 输出 一 个 前 馈 力 ,并 利用 加 权 最 小 二 乘法 来 估计 出 物体 的 边界 位 置 和 硬度 系数 。 

机 器 人 本 身 性 质 比较 复杂 , 且 其 系统 参数 在 很 多 实际 情况 中 具有 不 确定 性 或 时 变性 , 传 
统 的 控制 方法 都 需要 对 机 器 人 预先 了 解 ' 或 者 可 能 会 由 于 其 系统 参数 在 任务 过 程 中 发 生变 
化 而 限制 控制 效果 。 利 用 自 适应 控制 形式 简单 及 迭代 学 习 的 性 质 ,针对 系统 参数 不 确定 的 
机 器 人 模型 ,本 节 内 容 提 出 一 种 基于 迭代 自 适应 学 习 算法 ,在 有 效 时 间 内 学 习 双 近 实际 的 系 
统 参数 , 辅 以 简单 的 PD 控制 器 实现 良好 的 控制 。 


2.1.2. 背景 


1. 机 器 触觉 概述 

触觉 是 智能 机 器 人 实现 与 外 部 环境 直接 作用 的 必需 媒介 ,是 仅 次 于 视觉 的 一 种 重要 知 
觉 形式 ,与 视觉 不 同 , 触 觉 本 身 有 很 强 的 敏感 能 力 , 可 直接 测量 对 象 和 环境 的 多 种 性 质 特 征 。 
触觉 的 主要 任务 是 为 获取 对 象 . 环 境 信息 和 为 完成 某 种 作业 任务 而 对 机 器 人 与 对 象 . 环 境 相 
互 作 用 时 的 一 系列 物理 特征 量 如 形状 ,表面 纹理 、 硬 度 等 进行 检测 或 感知 。 机 器 人 触觉 与 视 
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觉 类 似 ,基本 上 是 模拟 人 的 感觉 ,广义 地 说 , 它 包 括 接触 觉 \ 压 觉 \ 力 觉 \ 滑 觉 \ 冷 热 觉 等 与 接 
触 有 关 的 感觉 ,狭义 地 说 , 它 是 机 械 手 与 对 象 接触 面 上 的 力 感觉 。 与 机 器 人 视觉 相 比 ,触觉 
仍 处 于 发 展 的 初级 阶段 。 机 器 人 触觉 的 研究 涉及 许多 理论 和 技术 ,如 计算 机 视觉 人 工 智 
能 ,心理 生理 学 、 传 感 器 技术 .控制 技术 .机 器 人 手 爪 技术 等 ,可 以 说 ,机 器 人 触觉 是 一 门 综合 
性 极 强 的 边缘 学 科 。 机 器 人 触觉 研究 的 主要 内 容 有 触觉 传感器 .触觉 控制 与 规划 、 触 觉 模 式 
识别 等 ,另外 还 有 系统 总 体 结构 以 及 手 爪 多 传感器 信息 融合 与 集成 技术 。 将 触觉 融合 于 视 
觉 ,将 为 智能 机 器 人 提供 可 靠 而 坚固 的 知觉 系统 。 

2. 机 器 触觉 发 展现 状 

目前 最 常见 的 机 器 人 触觉 识别 是 依靠 触觉 传感器 实现 的 。 机 器 人 和 触觉 传 感 技术 的 研究 
始 于 20 世纪 70 年 代 , 从 最 初 的 功能 简单 .探测 不 精准 的 简陋 模型 开始 ,逐步 发 展 到 现在 原 
理 运 用 材料 运用 、 功 能 设计 都 多 姿 多 彩 的 触觉 传感器 ,但 其 耐用 性 差 .通用 性 低 、. 可 靠 性 不 
足 一 直 是 其 不 可 忽视 的 缺陷 。 触 觉 技 术 相对 落后 的 原因 ,一 方面 是 现 阶 段 在 机 理 与 材料 上 
的 研究 不 足 , 另 一 方面 是 最 初 对 触觉 传感器 应 用 的 技术 与 市 场 定 位 不 当 。 尽 管 如 此 , 随 着 科 
学 的 快速 发 展 和 各 学 科 交 叉 频率 的 提升 ,触觉 传感器 的 发 明和 应 用 也 不 断 地 突破 各 种 难关 。 

近年 来 ,对 于 机 器 人 触觉 识别 的 实现 ,一 种 利用 力 反馈 的 力 触觉 方法 被 提出 并 研究 。 这 
种 方法 是 对 人 类 与 不 同 刚度 对 象 交 互 时 ,人 类 不 同 的 适应 行为 的 研究 。 研 究 表明 ,人 类 中 枢 
神经 系统 不 断 调节 相互 作用 力 来 抵消 一 个 兼容 的 对 象 ,同时 调节 参考 轨迹 来 围绕 一 个 刚性 
物体 并 避免 输入 过 大 的 力 。 

随 着 机 器 人 的 飞速 发 展 和 应 用 范围 的 扩大 ,机 器 人 的 触觉 识别 技术 也 需要 实现 更 精确 、 
更 多 样 化 的 目标 ,以 应 对 不 同 的 工作 环境 和 需求 。 力 触觉 识别 方法 摆脱 了 专门 的 触觉 传 感 
器 ,对 适用 的 机 器 拥有 更 强 的 兼容 性 ,虽然 仍 处 于 发 展 阶段 ,但 拥有 广大 的 发 展 前 景 。 

3. 机 器 触觉 识别 算法 概述 

一 般 来 说 ,探测 物体 表面 状况 和 物体 形状 ,可 以 单纯 地 依靠 触觉 ,或 者 加 上 视觉 进行 探 
测 , 旨 在 获取 被 测 物体 的 三 维 信息 或 硬度 弹性 (质地 )。 而 传统 的 触觉 探测 普遍 利用 专门 的 
触觉 传感器 ,其 优点 是 在 与 被 测 物体 交互 时 不 需要 输出 较 大 的 接触 力 。 但 有 一 些 特定 的 任 
务 , 如 抛光 或 挖掘 需要 对 物体 作用 一 个 较 大 的 力 ,甚至 会 渗透 进 表面 ,此 时 不 能 用 到 触觉 传 
感 器 。 另 外 ,因为 抛光 或 挖掘 的 同时 会 对 物体 表面 产生 影响 ,视觉 探测 对 于 这 种 变化 和 不 规 
则 性 无 法 提供 有 效 的 探测 ,在 有 些 光 线条 件 不 佳 或 者 视觉 死角 的 情况 下 ,视觉 探测 也 爱 莫 能 
助 。 于 是 ,摆脱 视觉 探测 .不 使 用 触觉 传感器 的 机 器 人 触觉 识别 方法 应 运 而 生 。 

为 了 控制 机 器 人 与 环境 的 相互 作用 , 现 已 提出 三 种 方法 : 力 / 位 置 混合 控制 ,阻抗 / 导 纳 
控制 和 力 /位 置 并 联 控制 。 第 一 种 方法 的 控制 过 程 可 以 分 为 接触 阶段 和 非 接触 阶段 ,两 阶段 
之 间 的 转换 需要 接触 面 的 精确 模型 ,而 不 精准 的 位 置 或 者 时 间 探 测 将 会 导致 转换 不 稳定 。 
第 二 种 方法 不 致力 于 追踪 需求 的 运动 和 力 , 而 更 偏向 于 利用 目标 阻抗 模型 来 调节 两 者 的 动 
态 变化 。 然 而 ,在 障碍 物 很 大 的 情况 下 .作用 于 物体 表面 的 力 会 随 着 与 参考 轨迹 之 间 的 距离 
增 大 而 增 大 ,而 这 可 能 会 损伤 到 物体 或 者 机 器 人 。 为 了 避免 以 上 问题 而 提出 了 第 三 种 方法 ， 
其 中 设计 的 力 控制 回路 要 比 位 置 控制 回路 更 占 优 势 。 但 是 ,这 个 策略 仍 需要 关于 力 所 作用 
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的 表面 几何 形状 的 信息 。 

总 的 来 说 , 当 任 务 对象 环 境 和 外 部 物体 已 经 建 模 得 比较 成 功 ,选择 一 种 传统 的 控制 器 就 
能 用 来 执行 任务 ,例如 .利用 阻抗 控制 并 谨慎 地 调节 阻抗 参数 能 完成 钻 孔 任务 ,利用 混合 控 
制 相 对 于 表面 的 水 平 位 置 和 垂直 作用 力 能 完成 抛 磨 任务 。 但 是 ,车 是 任务 环境 或 外 部 物体 
的 信息 提供 不 充足 ,这 些 机 器 人 策略 可 能 会 控制 失败 ,而 与 此 相对 的 是 ,人 类 在 没有 视觉 的 
情况 下 能 与 各 种 各 样 的 物体 相互 作用 。 这 启发 了 关于 人 类 如 何 与 未 知 的 利用 机 器 人 界面 产 
生 的 虚拟 力 场 的 动态 变化 进行 互动 的 研究 。 我 们 知道 ,在 自由 运动 中 人 类 中 枢 神 经 系统 
(Central Nervous System,CNS) 选 择 性 地 调节 肌肉 运动 来 保持 相同 的 稳定 裕 度 。 因 此 , 相 
对 于 人 类 CNS 调节 端点 的 作用 力 和 机 械 阻 抗 ,本 节 尝 试 建立 基于 学 习 算 法 的 机 器 人 触觉 识 
别 算 法 ,实现 力 和 轨迹 的 自 适应 ,控制 相互 作用 力 并 分 辨 出 物体 的 几何 形状 和 硬度 (弹性 ) 。 

4. 机 器 人 的 自 适应 控制 

由 于 自 适应 控制 对 受 控 对 象 的 系统 参数 的 先知 需求 较 小 , 较 于 其 他 传统 控制 方法 来 说 ， 
其 设计 更 加 简单 ,更 侧重 于 进行 迭代 学 习 , 不 断 地 修正 控制 器 中 的 自 适应 项 ,兴起 于 航空 领 
域 并 且 在 机 器 人 控制 方面 的 应 用 愈 来 愈 广泛 。 本 节 内 容 针 对 的 是 系统 参数 不 确定 的 机 器 人 
模型 ,所 以 也 采用 了 自 适应 迭代 学 习 控制 。 


2.1.3 算法 设计 


本 小 节 将 设计 一 个 参考 模型 ,其 参考 点 和 前 馈 力 在 迭代 域 上 进行 更 新 ,运用 加 权 最 小 二 
乘法 来 估计 被 测 物体 的 边界 和 弹性 ; 设计 一 个 自 适应 控制 器 ,其 更 新 律 是 在 时 域 上 更 新 的 ， 
用 来 保证 实际 系统 准确 跟随 参考 模型 ,由 于 参考 点 和 前 馈 力 在 每 个 周期 都 要 改变 ,需要 实际 
系统 与 参考 模型 之 间 的 跟随 误差 在 有 限时 间 内 趋 于 0, 采 用 FTCFinite Time) 跟 踪 , 即 跟随 
误差 w 在 有 限时 间 内 趋 于 0。 

1. 机 器 人 运动 学 及 动力 学 方程 

机 器 人 末端 执行 器 的 运动 学 方程 为 : 

xa) = $q) (2.1) 
其 中 ,x(t) 为 机 器 人 末端 执行 器 在 笛 卡 儿 空间 里 的 位 置 向 量 ,4 为 机 器 人 关节 角 向 量 ,是 一 
^P nX 1 维 向 量 ,n 是 机 器 人 的 自由 度数 量 。 本 节 内 容 后 续 仿真 是 在 一 个 双关 节 机 械 壁 上进 
行 的 , 即 n 二 2。 由 2.3 节 中 介绍 的 雅 可 比 矩 阵 , 将 笛 卡 儿 坐标 速度 与 关节 角速度 联系 起 
来 , 即 : 


T=J(g)g (2.2) 
对 上 式 求 导 得 : 
=J(g) q-Jap į (2.3) 
TEC C. DARO. 3) 代 入 机 器 人 动力 学 方程 式 (2. 1) 得 : 
MG) X C.(Q.d4) X -G,( =£ — fi (2.4) 


即 把 在 关节 和 角 空 间 里 的 动力 学 方程 对 应 到 笛 卡 儿 空间 里 。 
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其 中 : 
MG) X C,Q.Q) X G.(GD — $£ — fi 
M.G) —J7GMGJQG 


C.) = J 7 QY(CGQ. —MGDJ7 GD aI (D 
G.G) = JI (GG) 
=J "Ge FI fi—J 7 (Cg)gea 分 别 为 在 笛 卡 儿 空间 中 施加 给 末端 执行 器 的 力矩 和 来 
端 执行 器 受到 环境 作用 的 力矩 。M:-(qg) 也 是 对 称 且 正 定 矩 阵 。 
性 质 2.1 矩阵 2C:(4g,9 ) 一 M-(9g) 是 一 个 斜 对 称 正定 矩阵 。 
性 质 2.2 动力 学 方程 式 (2. 4) 的 右 侧 可 以 通过 参数 线性 化 表示 成 一 个 线性 回归 矩阵 
乘 以 未 知 参数 向 量 , 即 : 
Mi(q) X- CQ. X GG) — YG. x36 (2.5) 
Jep Y Ge x 30 d& — £f E E P. RR xx xt Xe oT — T Ut gn d oo Ue. Y 的 形 
式 是 已 经 固定 下 来 且 已 知 的 ; 8 是 执行 器 的 系统 参数 向 量 ,其 维 数 跟 系统 参数 的 数量 有 关 。 
2. 参考 模型 的 设计 
因为 机 器 人 末端 执行 器 的 物理 参数 是 未 知 的 ,我 们 设计 一 个 简单 的 参考 模型 如 下 : 
M,&- C,6€ Kane = f1— fa (2.6) 
HP eacus xa 是 在 笛 卡 儿 空间 里 的 需求 轨迹 ,也 称 参考 轨迹 ; M, LC, Kn 分 别 是 参考 
的 惯性 矩阵 .阻抗 矩阵 和 硬度 矩阵 。M Cu KS 的 选择 取决 于 其 应 用 情况 。 例 如 ,在 抛光 
任务 中 ,为 了 使 物体 表面 光滑 并 跟踪 要 求 的 轨迹 ,需要 在 工作 表面 垂直 方向 上 有 一 个 大 的 硬 
度 值 ,而 在 与 工作 平面 平行 的 方向 上 有 一 个 较 小 的 硬度 值 : 例如 在 本 节 内 容 中 , 想 让 机 器 人 
的 末端 执行 器 在 与 物体 的 交互 中 呈现 柔顺 状态 那么 就 需要 一 个 在 所 有 工作 方向 上 都 较 小 
的 硬度 值 。 
注意 : 参考 模型 式 (2.6) 给 出 了 位 置 误差 e 与 交互 力 f 1 之 间 的 动态 关系 。 在 菜 些 特殊 
任务 中 ,机 器 人 不 需要 与 物体 接触 , 即 接触 力 为 0, 若 考虑 式 (2. 6) 稳 定 ,那么 这 种 情况 下 实 
际 轨迹 将 会 收敛 到 与 参考 轨迹 重合 。 也 就 是 说 , 式 (2.6) 将 有 接触 和 无 接触 两 种 情况 结合 起 
来 ,这 意味 着 自由 运动 与 接触 运动 之 间 没 有 传输 ,这 一 点 很 重要 ,因为 实际 应 用 中 两 个 情况 
的 切换 往往 可 能 导致 语 动 ,其 至 会 破坏 系统 的 稳定 性 。 
3. 控制 目标 
本 节 内 容 的 最 终 目的 是 估计 出 接触 物体 的 边界 和 弹性 ,而 控制 目标 是 设计 一 个 控制 器 
控制 输入 力矩 w. 来 使 实际 系统 跟随 上 述 给 定 的 参考 模型 即 式 (2. 6) 。 建 立 一 个 误差 信号 将 
实际 系统 即 式 (2.4) 和 虚拟 系统 即 式 (2. 6) 联 系 起 来 .也 称 为 匹配 误差 ,其 表达 式 如 下 : 
w=—M,é—C,.e— Ketfi— fa (2.7) 
控制 目标 是 要 让 匹配 误差 w 趋向 于 0。 而 因为 控制 器 的 更 新 是 时 域 上 控制 每 个 周期 内 
机 器 人 的 运动 ; 参考 模型 中 的 参考 点 zs 和 前 馈 力 fs 的 更 新 是 在 迭代 域 上 ,而 这 要 求 在 每 个 
周期 中 实际 系统 都 与 参考 模型 匹配 .即使 w==0 成 立 , 那 么 我 们 设计 的 控制 器 不 仅仅 是 使 得 
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实际 系统 能 趋向 参考 模型 limw(1) 一 0, 而 且 是 要 在 有 限时 间 内 就 能 满足 , 即 当 1 二 4, 就 有 
W 一 0。 

因为 我 们 对 机 器 人 的 系统 知识 知之 甚 少 ,用 经 典 的 控制 方法 很 难 达 到 良好 的 控制 效果 ， 
因此 提出 采用 自 适应 控制 方法 , 且 为 了 使 匹配 误差 在 有 限时 间 内 收敛 ,参数 估计 的 更 新 律 采 
用 了 自 适应 有 限时 间 参 数 估计 。 

4. 控制 器 的 设计 

本 节 着 重 并 具体 地 介绍 控制 器 和 参数 估计 更 新 律 的 设计 。 为 了 后 续 分 析 设计 的 方便 ， 
根据 式 (2.7) 设 定 一 个 增长 的 匹配 误差 如 下 : 

w= Kw —— ë — Ka é — K,e - KCfi— fa) (2.8) 

其 中 ,Ks — MC, K, M, K,.K;—M,. 

A LL A dede sep ze dogm p eL ECCE. de (ib TE A ue UE VG o ix 25 
z, 其 表达 式 如 下 : 


:| Tg) rp (2.9) 


Kop PEDE EEKE e 是 实际 运用 中 的 w。 由 于 实际 测量 中 关节 角 的 加 速度 往往 不 可 
il e 的 表达 式 如 下 : 
6 =— Kié — Ke t- KiCfi— fa) (2. 10 
可 见 ,@ 比 w 少 了 关节 角 加 速度 这 一 项 。 
为 了 后 续 计 算 简 化 的 方便 ,重新 设计 < 为 : 
z=—ė+ e, +e (2. 115 
Je =L [Ert |. Lo (etel ,可 以 理解 为 分 别 是 高 通 和 低 通 滤波 的 
值 , 即 分 别 滤 去 频率 较 高 和 频率 较 低 的 变化 。 综 上 将 式 (2. 11) 代 回 式 (2. 8), 可 以 将 增长 匹 
配 误 差 # 表 示 成 = 十 Pz。 
从 式 (2. 11) 和 式 (2.8) 可 以 看 出 , 当 z —0 且 z—0 时 ,将 会 导致 w= 二 0。 
基于 学 习 算 法 的 柔顺 控制 设置 了 一 个 参考 模型 去 跟踪 匹配 ,用 了 自 适应 的 方法 实现 系 
统 参 数 的 估计 ,其 参数 估计 的 更 新 律 是 在 迭代 域 上 进行 的 。 参 考 控制 器 提出 了 计算 控制 输 
入 的 控制 器 如 下 : 


- 


ewe (2.12) 
Jtrp e Fedex FoU BCG MA e KARE es ees eet AAEH EJ E ER JI A RERUM 
偿 转 矩 矢量 ; 上 Si 的 测量 值 。 
注意 , 为 了 使 结果 能 应 用 得 更 加 灵活 ,假定 力 的 测量 精度 没 得 到 严格 的 保证 ,例如 存在 
着 一 个 力 的 测量 噪声 : AA MARAT ER. 
假设 1: 力 的 测量 噪声 是 有 界 的 , 且 其 界限 6 可 知 , 即 | 了 | 之 6。 
(D 实际 计算 中 ,计算 力矩 矢量 由 下 式 给 出 ， 
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e = Mt it Cute 6 -YGtatasb e (2.13) 
其 中 ,6 是 对 系统 参数 b 的 估计 ,运用 自 适 应 进行 更 新 : 
I = — e 
iat-34—d4a-d (2.14) 
(2) 反馈 力矩 矢量 的 表达 式 如 下 : 
t =— Kz* (2.15) 
其 中 ,K 是 一 个 对 称 正定 矩阵 。 
(3) 补偿 力矩 矢量 的 表达 式 如 下 : 
t — K,sgn(z^) (2. 16) 
其 中 ,Ks>6。 
£k E ,计算 力矩 矢量 赂 可 以 理解 成 为 抵消 系统 参数 变化 带 来 的 干扰 : 而 反馈 力矩 矢量 
金利 用 误差 进行 控制 ,类 似 于 PID 控制 中 的 PD 部 分 ; 而 补偿 力矩 和 撩 量 g; 用 来 补偿 因为 力 
测量 的 不 精确 带 来 的 误差 。 
将 上 述 控制 器 即 式 (2. 12) 整 合 进 机 器 人 末端 执行 器 在 操作 空间 动力 学 模型 即 式 (2.5) 
中 ,得 到 其 闭环 动力 学 方程 如 下 : 
M:t) z' -- C G* ,d02* + Kz: = 
Y G* 35, i* a 8 — Gosgn( — f (2.17) 
pe =o. 
5. 参数 估计 更 新 律 的 设计 
因为 在 每 次 试验 时 都 需要 更 新 参考 点 zi 和 前 馈 力 f4, 这 就 意味 着 前 面 的 匹配 误差 w 
应 该 在 有 限时 间 内 收敛 为 0, 即 实际 系统 在 有 限时 间 内 与 参考 模型 匹配 以 后 ,考虑 参考 点 zs 
和 前 馈 力 万 的 更 新 才 有 意义 。 为 了 保证 匹配 误差 w 应 该 在 有 限时 间 内 收敛 为 0, 用 另 一 种 
方法 设计 参数 估计 @ 的 更 新 律 。 
定义 替代 向 量 Falgi) =M: GP i RI H GI i40 7 — MG) i - C GI qa! + 
G. (q) ,根据 性 质 2.2 可 以 得 到 : 
Fx (g d) = M. (Git =Y qq 
H, (q.d) 2— M, G'O* +C G* d'y +G GP) (2.18) 
= Y; (gt g 
其 中 , (4d 00 Ys (q* ,4“)@* 都 是 新 的 回归 和 矩阵 ,可 以 看 出 ,两 者 都 与 关节 角 加 速度 YY 无 
关 。 式 (2.4) 可 以 表示 为 : 
FG d + BG — 2 fi (2. 19) 


Jb Fiat 40 CL q02-Y Gt o9. 
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EX Y: (d «900 Yo (f 9 00 和 时 经 过 一 阶 滤波 后 分 别 为 Yi (iq ERN, 
Y; (gq ,9  ) € R'" Figh € R" .JESc EJ fl Ys (a* 900" Y» Ct GOO 和 e? 的 曲线 更 平滑 ,有 
利于 后 续 运 算 控 制 : 


LY G 4D Yu (Gd) = Yla sgt) ,Yuy(g: ,0) |,0=0 


LYs Gf! d) +Y G* d) = Yala Gt) Yarat 49 | 一 0 


(2.20) 
1g v; = duo 
ff fs -o 
结合 式 (2. 18) 和 式 (2. 20) , 式 (2. 19) 可 表示 成 ， 
FG 十 HG Sg) = 
(Yu G' d - Ys G* 40 0 — 9$ — fs (2.21) 


将 式 (2. 20) 中 的 第 一 个 式 子 代入 上 式 得 : 


BOY hock 
[her D Yu yi wo vao = o oo 


s 284 B. o 
Jo qq BIG 90 YuG D. cy, cgt, ih ER"** 也 是 个 新 的 回归 矩阵 ,将 会 


用 在 参数 估计 更 新 律 的 设计 里 。 
定义 矩阵 PE RY 和 向 量 0*€ R* 如 下 : 


P ——|4p* YU Y$, P 0)= 0 
(2.23) 
Ò ——19* Y? Y$,0:(0)=0 
Kp 170 是 可 以 设计 的 参数 。 
定义 一 个 辅助 向 量 W*ERx ,其 可 以 根据 式 (2. 23) 中 的 Pr 和 Q@* 计 算得 到 : 
w = P 人 一 0 (2. 24) 
从 而 可 以 得 到 : 
w* = P — P'o —— P9 (2. 25) 
为 了 保证 参数 估计 的 误差 能 在 有 效 时 间 内 收敛 到 0, 选择 以 下 的 方法 : 
ê = a rw: (2. 26) 


Iwi 
其 中 ,A 70 是 一 个 增益 常数 矩阵 。 
选择 杰 雅 普 诺 夫 丽 数 的 形式 如 只 一 去 We p^" PO W, i DUI SU imw, 一 0 MATA 


限时 间 ta <2 VVO) /pe Vg Er Quis A — IP 9 Io V2 是 一 个 正定 的 标量 , 选 
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择 大 于 预先 设 定 的 常数 ; 5 9RO fi m HS PE Y, (qo g RW. y, = 
-f em yt GoftGOodr 也 有 界 。 
因此 , 当 >h 时 ,参数 误差 PF 可 以 看 成 0, 从 闭环 方程 即 式 (2.17) 可 以 看 出 , 当 @* 二 0 


时 ,Z=0 H Z=0, 那 么 匹配 误差 w= 二 0, 即 实际 系统 即 式 (2. 4) 准确 跟踪 了 参考 模型 即 
式 (2.6), 也 就 是 此 时 可 以 用 式 (2. 6) 代 替 式 (2. 4) 。 得 以 保证 后 续 的 分 析 在 式 (2. 6) 上 进行 。 
也 就 是 说 ,只 要 令 每 个 试验 周期 的 时 间 ya 且 始 终 令 下 一 个 迭代 周期 开始 的 时 候 参 数 的 
取 值 是 这 个 周期 最 后 时 刻 参 数 的 值 , 即 外 +1 C00 — 8^ Cro ,就 可 以 在 第 k=0 个 周期 内 完成 
机 械 臂 不 确定 参数 的 学 习 , 从 第 &= 1 个 周期 开始 ,就 能 够 开始 估计 物体 的 边界 和 弹 
性 了 。 
6. 参考 点 和 前 馈 力 的 自 适应 
假设 在 每 次 试验 中 C。、K。 和 初始 参考 点 zx, 都 选择 恰当 ,使 得 末端 执行 器 得 以 平滑 地 
接触 并 触 碰 未 知 物体 而 不 会 毁坏 它 。 阻 尼 系 数 Cw 是 用 来 避免 过 大 的 速度 和 反弹 ,硬度 系 
数 KG, 用 来 产生 一 个 柔顺 的 驱动 力 来 跟随 参考 点 zs 参考 点 的 初始 值 为 在 物体 边界 内 部 的 
Zz。 用 zc 来 表示 机 器 人 末端 执行 器 在 试验 中 首次 触 磁 物 体 的 边界 所 处 的 位 置 ,用 Ko 表示 
物体 的 硬度 ,那么 在 接触 过 程 中 ,由 于 物体 弹性 所 产生 的 作用 在 机 器 人 末端 执行 器 的 反作用 
力 可 以 表示 成 : 
万 一 开 o(z 一 Zo) (2. 27) 
其 中 ,K。 是 代表 着 物体 的 硬度 ,而 zxs 是 未 端 执 行 器 在 表面 变形 前 的 接触 位 置 。 
由 之 前 的 讨论 可 知 , 当 >t 时 ,匹配 误差 w= 二 0。 由 此 可 以 将 式 (2. 27) 代 入 参考 模型 即 
式 (2.7) 中 ,得 到 接触 过 程 中 的 闭环 动态 方程 : 
Mnée - C, e + Kye — Ko(x — x4) —— fa (2. 28) 
取 每 次 试验 的 时 间 为 ty, 并 假设 这 个 时 间 足 够 大 ,使 得 在 这 个 时 间 段 内 机 器 人 末端 与 物 
体 交 互 已 达到 平衡 点 +. 。 综 合 上 面 的 W 在 有 效 时 间 4。 内 收敛 ,可 知 ty 与 1。 的 关系 应 为 ， 
tt。 
在 平衡 点 位 置 时 有 : 
Ko(z。 — x4) =— Kr. — Ta) + fa (2. 29) 
1l v — 2^ ah RESO Hs 和 前 馈 力 fa AB E 次 到 & 十 1 次 试验 的 更 新 规律 如 下 : 
j = 2} = z, k = 2,3,4, 


a5 = rh tav + A — at) Cr arh) <0) (2. 30) 
rf = rh-c-(0—a0Q,.—a5 (Q0) 
f= fh = Fsk = 2.34 
b = ff + Ka — 7) -gG,— f. GA 0) (2. 31) 
f = AHE GO 
其 中 ,0<<a* 二 1 是 一 个 柔顺 因子 。a 越 大 ,参考 点 将 越 随 着 物体 而 变化 ; 反之 a 越 小 ,参考 点 
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的 改变 值 将 越 小 , 则 机 器 人 更 趋向 于 保持 上 次 的 参考 点 。 人 类 运动 控制 实验 表明 , 当 人 类 主 
体 与 一 个 柔软 的 物体 交互 时 ,人 类 使 用 一 种 补偿 性 响应 ,趋向 于 在 自由 运动 中 遵守 参考 点 ; 
而 当 物 体 比 较 硬 时 ,它们 将 跟随 着 物体 的 边缘 来 修改 参考 点 。 由 这 个 观察 现象 得 出 启示 : 
根据 对 物体 刚度 的 估计 来 选择 一 个 柔性 的 a, 这 将 会 在 式 (2.37) 中 说 明 ; 09 —1 是 个 放 
松 因子 , 它 更 倾向 于 使 fa 的 值 趋向 于 Fo( 默 认 的 接触 力 )。 这 个 可 能 与 人 类 运动 控制 中 的 
疲劳 相 一 致 。 当 人 举 着 一 个 高 硬度 的 物体 (如 玻璃 杯 ) 时 ,他 得 小 心经 翼 地 控制 接触 力 以 防 
摔 坏 它 ,而 当 物 体 比较 有 弹性 更 容易 拿 住 时 ,他 就 能 够 控制 得 比较 轻松 。 所 以 ,应 当 设 定 这 
个 因数 随 物体 刚度 的 增 大 而 减 小 ,正如 式 (2.37) 中 所 示 。 

当 v0 时 , 换 句 话说 ,机 器 人 末端 执行 器 到 达 的 地 方 超过 了 参考 设置 点 ,这 也 许 是 额 
外 的 前 馈 力 导致 的 ,或 者 由 于 物体 的 刚度 降低 了 ,如 物体 已 被 移 走 或 者 被 替换 成 一 个 比较 柔 
软 的 东西 。 在 前 面 一 种 情况 中 ,在 式 (2. 31) 中 将 会 减少 前 馈 力 ; 然后 在 后 面 一 种 情况 中 ， 
将 简单 地 调节 zs 回 到 原来 的 参考 点 ,例如 当 物 体 被 移 走 时 ,机 器 人 末端 执行 器 应 归 回 
原状 。 

7. 计算 物体 的 边界 和 弹性 

为 了 推导 方便 ,定义 一 些 变 量 如 下 : 


h = 02x 
TENE C 
di ce 

4 
LN d ko 
" — Kelah — x) ls f= xi c (2.32) 

将 上 式 代 入 动态 闭环 方程 即 式 (2. 28) 中 并 化 简 得 : 
ea - s (2.33) 


其 中 ,9 一 [0 ,0:]7,G 一 [加 加] 。 
因为 式 (2. 33) 的 系数 是 线性 的 ,可 以 用 一 些 求解 方法 如 梯度 下 降 或 者 递归 最 小 二 乘法 
来 估计 位 置 参数 0 和 0。 为 了 获得 比较 快速 的 收敛 性 能 ,采用 加 权 最 小 二 乘法 : 


& = e e L'o 8 o» 


Lt e P'o* 
art AT pig 
o? 4- 9! Pho 
kk! pk 
pel peo PO Poo (l3) (2. 34) 
o? 4-9 Po 


Jepe =C AT rtt Âi Â; 4380 8 0. 0 的 估计 值 。 那 么 在 第 次 实验 时 ,Ko、z 的 估计 
值 分别 用 义 4、 答 表示 ,为 : 
Ks E E. d = à Ce* «C 0) 
01 (2. 35) 


Éi-gKp uma db --ü-ar (t0 
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从 数学 上 来 讲 ,6" 的 O 的 初始 估计 值 能 任意 取 值 ,但 实际 上 一 般 选择 K。 和 zs 的 初始 
值 分 别 为 二 KI 二 K。 和 各 二 zx,; 其 中 Ko 是 物体 刚度 可 能 的 最 大 值 ,而 x, 是 机 器 人 末端 
执行 器 的 初始 位 置 。 另 外 , 式 (2. 34) rf P 的 初始 值 可 简单 地 设 为 已 一 工 ,其 中 工 是 单位 矩 
阵 , 权 值 序列 o 为 
o r (2. 36) 
log? (14- >) Ila 
此 时 ,Ko 已 经 估计 出 来 ,现在 计算 式 (2. 30) 和 式 (2.31) 中 的 柔顺 因子 o^ 和 放松 因子 B4， 


AR p'—1—a* (2. 37) 
其 中 必 是 一 个 由 设计 者 设计 的 常数 。 
2.1.4 实验 设计 
根据 算法 设计 可 以 得 到 触觉 识别 算法 实现 框图 如 图 2-1 所 示 。 本 节 将 对 实验 中 三 个 部 


分 进行 介绍 ,包括 如 何 使 用 机 器 人 工具 箱 对 Baxter 机 器 人 进行 建 模 ,Simulink 中 的 机 器 人 
模块 和 加 权 最 小 二 乘法 的 实现 。 


H 参数 估计 


"ECL TN m 
neni e REND een 


图 2-1 触觉 识别 算法 实现 框图 


1. Baxter 机 器 人 模型 的 建立 

使 用 MATLAB 机 器 人 工具 箱 对 Baxter 机 器 人 进行 运动 学 和 动力 学 建 模 , 生 成 的 可 视 
化 模型 如 图 2-2 所 示 。 这 里 以 Baxter 机 器 人 的 左 臂 建 模 为 例 , 所 使 用 的 MATLAB 代码 
如 下 : 


= 
% 1. left s0 --> left upper shoulder --> left sl 
% Left(1) = Link([ 0 Lzsl Lxsl - pi/2 0 ], 'standard'); Left(1).offset = 0; *sl 
ixx = 0.04709102262; ixy= 0.00012787556; ixz - 0.00614870039; 

iyy = 0. 03766976455; iyz = 0.00078086899; 

izz = 0. 03595988478; 

I_interia = [ixx iyy izz ixy iyz ixz]; 
% \Theta d a \alpha \sigma 
Left(1) = Link([0, left sl xyz(3), left sl xyz(1), -pi/2, 0 ], 'standard'); 
Left(1).offset = 0; 
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Left(1).qlim = [一 1.70167993878 1.70167993878]; % [—97.5, 97.5], left .s0 
Left(1).I 7 I interia; 

Left(1).r = [0.01783 0.00086 0.19127] + [0,0,0]; % left s0 = joint s0 
Left(1).m = 5.70044; 

Left(1).Jm - 200e-6; 

Left(1).G - -62.6111; 

Left(1).B = 1.48e- 3; 

Left(1).Tc = [0.395 - 0.435]; 


* 2. left sl -一 > left lower shoulder -- ^ left e0 
% Left(2) = Link([000 pi/2 0 ], 'standard'); Left(2).offset = pi/2; 
ixx- 0.01175209419; ixy- — 0. 00030096398; ixz = 0. 00207675762; 
iyy = 0. 0278859752; iyz- — 0.00018821993; 
izz = 0. 02078749298; 
I_interia = [ixx iyy izz ixy iyz ixz]; 


* XTheta d a Nalpha \sigma 

Left(2) = Link([0, 0, 0, pi/2, 0 ], 'standard') ; 
Left(2).offset - pi/2; 

Left(2).qlim = [-2.147 1.047]; & [—123, 60], left s1 
Left(2).I 7 I interia; 

Left(2).r = [0.06845 0.00269 —0.00529] + [0,0,0]; $ left sl = joint sl; 
Left(2).m = 3.22698; 

Left(2).Jm - 200e-6; 

Left(2).G = 107.815; 

Left(2).B = .817e-3; 

Left(2).Tc = [0.126 -0.071]; 


* 3. left e0 -一 > left upper elbow -一 > left el 
$ Left(3) = Link([ 0 Lxe0 + Lzel Lxel — pi/2 0 ], 'standard'); Left(3).offset = 0; 
ixx = 0.02661733557; ixy = 0.00029270634; ixz = 0.00392189887; 
iyy = 0.02844355207; iyz = 0.0010838933; 
izz = 0. 01248008322; 
I_interia = [ixx iyy izz ixy iyz ixz]; 


% \Theta d a \alpha \sigma 

Left(3) = Link([0, left_e0_xyz(1) + left_el_xyz(3), left el xyz(1), - pi/2, 0], 

'standard'); 

Left(3).offset - 0; 

Left(3).qlim = [-3.05417993878 3.05417993878]; % [—175, 175], left e0 

Left(3).I 7 I interia; 

Left(3).r = [- 0.00276 0.00132 0.18086] + [0,0, left eO xyz(1)]; * left eO != 
* joint eO 

Left(3).m = 4.31272; 

Left(3).Jm = 200e- 6; 

Left(3).G 175301061; 

Left(3).B = 1.38e- 3; 

Left(3).Tc = [0.132, — 0.105]; 


% 4. left el ——» left lower elbow --» left wO 
* Left(4) = Link([ 0 0 0 — pi/20 ], 'standard'); Left(4).offset = 0; 
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ixx= 0.00711582686; ixy= 0.00036036173; ixz= 0.0007459496; 


I interia = [ixx iyy 


iyy = 0.01318227876; iyz= — 0.00019663418; 
izz = 0. 00926852064; 


izz ixy iyz ixz]; 


% \Thetad a \alpha \sigma 


Left(4) = Link([ 0, 
Left(4).offset = 0; 
Left(4).qlim = 
Left(4). I = 
Left(4).r - 
Left(4).m - 
Left(4).Jm =z 
Left(4).G = 
Left(4).B = 
Left(4).Tc = 


0, 0, pi/2, 0], 'standard'); 


[ - 0.05 2.618]; * [- 2.865, 150], left el 
I interia; 

[0.02611 0.00159 -0.01117]+ [0,0,0]; % left el = joint el 
2.07206; 

BSe 6/ 

76.0364; 

Te cc 

[11:2e—3, —16.9e— 3]; 


% 5. left w0 一 一 > left upper forearm --> left wl 
% Left(5) = Link([ 0 LxwO * Lzwl Lxwl ~ pi/2 0 ], 'standard'); Left(5).offset = 0; 
ixx = 0.01667742825; ixy= 0. 00018403705; ixz = 0.00018657629; 


I_interia = [ixx iyy 


iyy = 0.01675457264; iyz- - 0. 00064732352; 
izz = 0. 0037463115; 
izz ixy iyz ixz]; 


% \Theta d a \alpha \sigma 


Left(5) = Link([0, 
'standard') ; 

Left(5).offset = 0; 
Left(5).qlim 
Left(5).I = 
Left(5).r = 
Left(5) .m = 
Left(5).Jm 三 
Left(5).G = 
Left(5).B = 
Left(5).Tc = 


left w0 xyz(1) + left wl xyz(3), left wl xyz(1), - pi/2, 0], 


[ -3.059 3.059]; % [—175.27, 175.27], left w0 

I interia; 

[ - 0. 00168 0.0046 0.13952] + [0,0, left wO xyz(1)]; * left w0 = joint wO 
2.24665; 

33e- 6; 

71.923; 

82.6e- 6; 

[9.26e-3, -14.5e-3]; 


% 6. left wl ——» left lower forearm --» left w2 
% Left(6) = Link([00 0 pi/2 0 ], 'standard'); Left(6).offset = 0; 
ixx = 0.00387607152; ixy- — 0. 00044384784; ixz = — 0.00021115038; 


I interia - [ixx iyy 


iyy = 0.00700537914; iyz = 0.00015348067; 
izz = 0. 0055275524; 
izz ixy iyz ixz]; 


% \Theta d a \alpha \sigma 


Left(6) = Link([0, 
Left(6).offset = 0; 
Left(6).qlim = 
Left(6).I 
Left(6).r 


07207 pi/2, 0], 'standard'); 


[-1.57079632679 2.094]; % [-90, 120],left wl 
I interia; 
[0.06041 0.00697 0.006] * [0,0,0]; % left w1 = joint w1; 
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Left(6).m = 1.60979; 

Left(6).Jm - 33e-6; 

Left(6).G = 76.686; 

Left(6).B = 36.7e—6; 

Left(6).Tc = {3:965=-3,;, =10.56=3]; 


% 7. left w2 —» left wrist 一 一 > 
% Left(7) = Link([ 0 Lxw2 0 0 0 ], 'standard'); Left(5).offset = 0; 
ixx = 0.00025289155; ixy- 0. 00000575311; ixz- - 0.00000159345; 

iyy- 0.0002688601; iyz- — 0. 00000519818; 

izz- 0.0003074118; 

I interia - [ixx iyy izz ixy iyz ixz]; 
* NTheta d a \alpha \sigma 
Left(7) = Link([0,left w2 xyz(1) + left_wrist_hand xyz(3) + left endpoint xyz(3), 0, 0, 0], 
'standard'); 
Left(7).offset = 0; 


Left(7).qlim = [- 3.059 3.059]; $ [-175.27, 175.27], left w2 

Left(7).I = I interia; 

Left(7).r = [0.00198 0.00125 0.01855] + [0,0, left w2 xyz(1)]; * left w2 !- joint w2 
Left(7).m = 0.35093 + 0.19125 + 0.0001 * 4; % + hand + camera + range + base + gripper 
Left(7).Jm = 33e-6; 

Left(7).G = 76.686; 

Left(7).B = 36.7e-6; 

Left(7).Tc = [3.96e-3, = 10.5e- 3]; 


LeftArm = SerialLink(Left, 'name', 'LeftArm'); 
LeftArm.base = ... 
transl(base torso xyz(1), base torso xyz(2), base torso xyz(3)) ... 
* transl(torso left arm mount xyz(1), torso left arm mount xyz(2), torso left arm. 
mount xyz(3)) ... 
* trotz(torso left arm mount rpy(3)) ... 
* transl(left s0 xyz(1),0,left s0 xyz(3)); 


图 2-2 Baxter 机 器 人 模型 
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2. Simulink 中 的 机 器 人 模块 


在 Simulink 中 ,实现 的 算法 是 对 机 器 人 进行 控制 ,其 中 机 器 人 模块 的 Simulink 框图 如 
图 2-3 所 示 。 


Scopes 
图 2-3 机 器 人 模块 的 Simulink 框图 
在 机 器 人 模块 中 ,slaccel 模块 的 代码 如 下 : 


* SLACCEL S- function for robot acceleration 
* 


* This is the S - function for computing robot acceleration. It assumes input 
data u to be the vector [q qd tau]. 
* 


* Implemented as an S - function to get around vector sizing problem with Simulink 4. 


function [sys, x0, str, ts] = slaccel(t, x, u, flag, robot) 
Switch flag, 


case 0 

% initialize the robot graphics 

[sys,xO,str,ts] = mdlInitializeSizes(robot); % Init 
case (3] 


* come here to calculate derivitives 


% first check that the torque vector is sensible 
if numel(u) ~ = (3 robot.n) 


error('RTB:slaccel:badarg', 'Input vector is length % d, should be &d', numel(u), 
3 * robot.n); 


end 


if —isreal(u) 
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3. 加 权 最 小 二 乘法 的 实现 
在 计算 物体 的 边界 和 弹性 中 ,为 了 获得 比较 快速 的 收敛 性 能 ,采用 加 权 最 小 二 乘法 来 估 
计 位 置 参数 负 和 0。。 使 用 Simulink 对 加 权 最 小 二 乘法 进行 实现 ,如 图 2-4 所 示 。 


四 
uM (u1«-1 35) f 
Merge 


Constant M Action 
Subsystem1 


图 2-4 加 权 最 小 二 乘法 的 Simulink 框图 


2.1.5 实验 与 结果 


本 节 将 在 MATLAB/Simulink 平台 中 ,利用 已 有 的 机 器 人 工具 箱 里 的 机 器 人 模型 ,对 
控制 器 进行 仿真 。 本 节 所 提 到 的 控制 器 的 实现 大 体 可 分 为 两 个 部 分 ,第 一 部 分 先进 行 自 适 
应 参数 估计 ,在 有 限时 间 内 参数 估计 值 收敛 后 , 即 实际 系统 即 式 (2. 4) 与 参考 模型 即 式 (2. 6) 
成 功 匹配 后 ,再 通过 调节 参考 点 和 前 馈 力 ,并 利用 最 小 二 乘法 对 物体 边界 和 弹性 进行 估计 和 
学 习 。 由 于 程序 较 复杂 且 需 调 参数 较 多 ,在 实现 过 程 中 先 把 时 域 和 迭代 域 分 开 来 调试 实现 ， 
一 方面 设计 一 个 简单 的 PD 控制 器 控制 机 器 人 与 被 测 物体 接触 ,每 次 试探 时 间 为 3s, 共 试探 
20 次 ,每 次 试探 结束 就 计算 更 新 参考 点 和 前 馈 力 , 并 估计 出 被 测 物体 的 性 质 ; 另 一 方面 , 实 
现实 际 系 统 与 参考 模型 在 有 限时 间 内 匹配 ,才能 用 上 前 面 的 物体 估计 部 分 。 把 上 述 第 一 部 
分 又 再 细 分 : 先 将 系统 参数 0 设 定 为 真实 值 , 同 时 调节 参数 KK 使 系统 保持 稳定 状态 ; 再 加 
上 参数 估计 部 分 6 使 匹配 误差 w 在 有 限时 间 内 收敛 。 另 外 ,为 了 分 析 简便 ,本 节 内 容 仿真 所 
目的 是 较为 简单 的 双关 节 单 机 械 臂 ,具有 两 个 自由 度 , 所 触 碰 物 体 设 定 为 一 个 弹簧 ,可 通过 
设置 改变 其 末端 位 置 zs 及 其 劲 度 系数 玉 。, 只 研究 执行 器 与 弹簧 在 c 轴 方 向 的 交互 ,其 位 置 
关系 如 图 2-5 所 示 。 

H milis Ls G—1.2)2: EAR ORT = 轴 的 质量 .长度 . 惯 性 和 关节 重心 与 上 一 个 关 
节 之 间 的 距离 ,其 中 = 轴 方 向 为 穿 出 纸 面 的 方向 。 令 ma 二 ms 二 10. 0kg. — 1 — 1. 0m. Ti 
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图 2-5 双关 节 单 机 械 臂 与 被 测 弹 簧 


1;—0. 83kgm*. [a = le — 0. lm。 注 意 , 这 里 介绍 的 参数 是 为 了 搭建 仿真 平台 ,因为 
MATLAB 中 的 机 器 人 模块 也 需要 设置 一 些 参 数 才能 使 用 ,并 没有 用 到 前 文 设计 的 控制 器 
中 。 本 实验 假设 机 械 臂 所 受 的 重力 因素 G(q) 一 0。 
如 式 (2.7) 所 示 ,采用 如 下 简写 方式 : sı — sinl) ,ss 二 sin(q2) ,$1 二 sin(qi 十 qz) ,cl 
cos(qi) ,cz 一 cos(qz),ciz 一 cos(qi 十 qz) ,该 机 械 臂 的 雅 可 比 矩 阵 为 : 
— (siclesig) lzsi2 
hec lzen 


Jp = 


机 械 臂 未 端 执行 器 的 初始 位 置 为 : 
a*(0)— l.0m,  y*(0)— 0. 0m 
设 定 每 次 试探 时 间 为 3s,3s 内 机 器 人 与 被 测 弹 簧 之 间 已 处 于 稳定 的 平衡 关系 , 即 已 到 
达 平 衔 点 zx.。 弹 簧 的 劲 度 系数 设 为 Ko=150, 边 界 所 处 位 置 为 ze 王 1.3。 机 器 人 未 端 执行 器 
的 初始 位 置 z, 王 1。 自 适应 参数 估计 机 器 人 的 参数 结果 如 图 2-6 所 示 ,实际 系统 与 参考 模型 
的 匹配 误差 如 图 2-7 所 示 。 


Ls] 
| De PSI AL. n 
os| 
& of 
-05 
"| 
一 1.5| 
749 38 330 Is 29 25 30 719. 357 10 5 20 25 30 
图 2-6 自 适 应 参数 估计 机 器 人 的 参数 结果 图 2-7 实际 系统 与 参考 模型 的 匹配 误差 
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在 学 习 好 的 基础 上 ,经 过 20 次 迭代 探测 物体 后 ,将 估计 的 劲 度 系数 和 边界 位 管 以 曲线 


形式 表示 出 来 ,如 图 


2-8 所 示 。 可 以 看 出 ,从 第 4 个 周期 开始 已 经 能 准确 地 估计 出 实际 弹簧 


的 Ko 和 xs。 说明 所 用 的 加 权 最 小 二 乘法 确实 能 保证 收敛 的 快速 性 。 为 了 证 明 程序 的 可 靠 
估计 和 非 偶 然 性 ,我 们 另外 设 弹簧 的 劲 度 系 数 设 为 Ko 王 200, 边 界 位 置 为 zs 王 1.4 LB E 
数 设 为 Ko 王 175 ,边界 位 置 为 zs 二 1.2, 测 量 估计 曲线 结果 分 别 如 图 2-9 和 图 2-10 所 示 。 可 
以 看 出 ,就 算 弹 簧 性 质 发 生 改 变 ,程序 也 能 成 功 准确 地 估计 出 其 硬度 和 边界 性 质 。 
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图 2-8 K,—150.2,—1.3 
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图 2-9 K,—200,2,—1.4 
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图 2-10 
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次 数 


Ko=175,x,=1.2 


2.2 基于 波动 变量 和 神经 网 络 的 远程 控制 系统 


本 节 将 介绍 基于 波动 变量 和 神经 网 络 的 远程 操作 系统 的 数学 模型 .原理 ,编程 与 实现 。 
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23.2.1 Sla 


在 过 去 的 几 十 年 中 ,机 器 人 技术 在 各 种 工程 领域 中 得 到 快速 发 展 。 远 程 操作 作为 机 器 
人 技术 中 最 具 吸 引力 和 最 具 挑 战 性 的 话题 之 一 ,被 应 用 于 远程 外 科 、 搜 索 和 救援 .三 维 游戏 
开发 等 各 种 应 用 。 典 型 的 远程 操作 系统 通常 包括 五 个 部 分 : 人 类 操作 者 、 主 设备 .通信 信 
道 .从 机 器 人 、 环 境 。 通 常 ,人 类 操作 者 控制 与 人 接触 的 主 设备 进行 运动 ; 随后 主 设备 生成 
对 应 的 命令 轨迹 通过 通信 信道 传递 ; 接 下 来 ,命令 轨迹 被 传递 到 作用 于 目标 环境 并 执行 某 
任务 的 从 属 机 器 人 。 从 机 器 人 和 环境 之 间 的 相互 作用 力 被 反馈 到 主机 ,根据 该 相互 作用 力 ， 
人 类 操作 者 可 以 更 有 效 地 控制 机 器 人 。 

本 节 使 用 Geomagic Touch X 作为 主 设备 。Touch X 由 SensAble Technologies Inc. 设 
计 。Touch X 是 一 种 包括 硬件 驱动 器 和 软件 包 (OpenHaptics 工具 包 ) 的 触觉 反馈 设备 。 
Touch X 臂 包括 三 个 旋转 关节 ,每 个 旋转 关节 配备 有 产生 反馈 力 的 电机 。 

作为 从 机 器 人 ,在 Baxter 的 每 个 臂 中 有 7 个 旋转 关节 ,这 使 得 它 在 3D 空间 中 容易 移 
动 。 为 了 抓 握 和 处 理 物 体 ,在 每 个 臂 的 末端 执行 器 处 安装 旋转 夹具 。MATLAB 机 器 人 工 
具 箱 用 于 建立 Baxter 机 器 人 手臂 的 运动 学 和 动力 学 模型 ,用 作 从 属 远程 机 器 人 来 测试 所 提 
出 的 方法 。 

通信 通道 在 远程 操作 系统 中 起 重要 作用 ,并 且 通 道中 的 时 间 延 迟 可 能 在 存在 力 反馈 
的 情况 下 导致 系统 不 稳定 。 由 于 实际 应 用 中 存在 不 确定 性 ,控制 不 确定 机 器 人 系统 的 研 
究 变 得 非常 重要 。 机 器 人 的 自 适应 控制 方法 已 经 在 相当 多 的 工作 中 进行 了 研究 ,可 用 于 
机 器 人 模型 中 未 知 参数 和 时 变 参 数 的 情况 。 机 器 人 模型 的 时 变 延 迟 和 不 确定 性 已 经 与 
自 适应 控制 一 起 被 研究 。 近 年 来 ,由 于 神经 网 络 具 有 仿真 复杂 的 非 线性 和 不 确定 的 函数 
的 能 力 , 神 经 网 络 ( 神 经 网 络 ) 在 机 器 人 控制 系统 中 的 应 用 越 来 越 普及 。 径 向 基 责 数 
(Radical Basis Function,RBF) 神 经 网 络 是 一 种 非常 有 效 的 方法 ,已 广泛 用 于 不 确定 机 器 人 
系统 的 控制 设计 。 自 适应 RBF 基于 神经 网 络 的 控制 已 经 被 用 于 处 理 死 区 和 不 确定 的 机 器 
人 模型 。 

本 节 基 于 PD 控制 的 神经 网 络 控制 器 应 用 于 具有 7 个 自由 度 的 从 机 器 人 ,这 保证 了 比 
常规 PD 控制 器 更 准确 的 轨迹 跟踪 。 


2.2.2 远程 操作 系统 的 数学 模型 


1. 远程 操作 系统 的 框架 

远程 操作 系统 如 图 2-11 所 示 . 操 作者 握 住 触觉 装置 的 接触 杆 并 驱动 主 装置 运动 ,这 提 
供 了 位 置 命令 。 通 过 计算 机 的 处 理 , 新 命令 将 由 主 计算 机 生成 ,然后 传递 到 从 计算 机 ,并 由 
从 控制 器 接收 。Baxter 机 器 人 将 根据 从 控制 器 的 命令 移动 。 操 纵 器 的 末端 执行 器 与 环境 
相互 作用 ,并且 相 互 作用 力 被 传递 到 触觉 装置 已 由 操作 者 感 测 ,这 将 导致 新 的 运动 和 新 的 控 
制 命令 。 以 下 分 析 远 程 操作 系统 的 各 个 组 件 及 其 数学 模型 。 
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EO 


图 2-11 系统 的 框架 


2. 主机 器 人 和 臂 的 运动 学 和 动力 学 

力 触觉 操纵 杆 Geomagic Touch X 是 一 种 6 关节 机 器 人 ,是 对 人 的 手臂 进行 模仿 相应 
设计 的 ,触觉 设备 Touch X 不 仅 向 主 设备 发 送 运动 命令 ,而 且 返 回 远程 机 器 人 与 环境 之 间 
的 相互 作用 力 , 这 对 于 操作 者 调节 接触 力 是 非常 有 用 的 。 主 机 器 人 的 数学 模型 包括 运动 学 
模型 和 动力 学 模型 。 

Touch X 的 运动 学 模型 是 基于 其 结构 ,如 图 2-12 所 示 。Touch X 具有 6 个 旋转 关节 ， 
其 中 3 个 关节 配备 有 电动 机 ,而 另外 3 个 被 认为 是 末端 执行 器 的 万 向 节 关 节 , 使 其 可 以 灵活 
地 在 工作 空间 内 移动 。 


| an 


b z i 0 z - 
4 - zn M. 3 Ln ER 
^» z V3 L 
6, "y Yw 
i xj Xy 
[! *" 
d,| - Oy zu 
' d. 
H el 


图 2-12. Touch X 的 结构 


为 了 更 具体 和 直观 地 表示 其 结构 ,DH 参数 用 于 建立 运动 学 模型 。DH 参数 有 两 种 表 
示 形 式 : 

* 标准 的 DH 参数 法 ; 

。 修改 的 DH 参数 法 。 

后 面 将 根据 修改 的 DH 参数 法 用 于 本 节 中 的 Touch X 操纵 杆 的 运动 学 建 模 。 根 据 标 
HERI DH 参数 法 ,如 图 2-13 的 左 图 所 示 ,与 关节 4 和 5 相关 的 坐标 的 原点 是 相同 的 。 

因此 ,应 该 修改 由 MATLAB 机 器 人 工具 箱 建 模 的 仿真 机 器 人 。 具 体 地 ,ai_1 和 d; 分 别 
用 于 表示 连 杆 长 度 和 连 杆 偏 移 , 其 中 i 表示 主 设备 的 第 i 个 关节 。ai_1 和 0; 分 别 用 于 表示 关 
节 扭 转角 和 关节 角 。 所 有 6 个 主 设备 的 关节 是 旋转 的 ,并 且 Touch X 的 修改 DH 参数 在 
X 2-1 中 获得 。 表 2-1 中 的 DH 参数 表示 主 设备 的 结构 特性 ,从 其 中 可 以 获得 运动 学 模型 。 
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依据 标准 的 DH 参数 法 建立 的 模型 依据 修改 的 DH 参数 法 建立 的 模型 


0 0 0 
4 
y? 04 o4 92, y 05705 x 


(a) 基于 标准 DH 参 数 (b) 修改 的 DH 参 数 
图 2-13 Touch X 的 两 个 运动 学 模型 之 间 的 比较 


R21 EHH DH 参数 


Link i 0, Cangle limit(deg)) di ai-i ai~ı (rad) 
1 qı (—60~60) 0 0 0 
2 gz(0 一 105) 0 0 一 /2 
3 g3( 一 180 一 180) 0 La 0 
4 qa C7 145—145) | 0 —x/2 
5 gs (—70~70) 0 0 —x/2 
6 qs C— 145—145) 0 0 一 K/2 


两 个 相 邻 坐标 之 间 的 齐 次 变换 ,可 以 使 用 DH 参数 表示 如 下 : 


ch; — sÜicai SOicai aichi 
chi chica; — cÜica; aishi 
HIA; (B; d; «a; sai) = (2. 38) 
0 ai cai di 
0 0 0 1 


其 中 ,c 是 三 角 函 数 cos 的 缩写 ,* 是 sin 的 缩写 。 
此 外 ,可 以 用 下 式 来 表示 末端 执行 器 的 位 置 和 基部 之 间 的 关系 : 
"Xo —? A, Az "A,n 。X, (2. 39) 
其 中 ,对 于 主 设备 nn 为 6,X — [x,y,z,1] 表 示 相 关 关 节 的 位 置 ,Ain(i — 0,1,…,n 一 1) 表 
示 相 邻 的 坐标 。 
主机 器 人 臂 的 动力 学 模型 表示 了 驱动 转 抢 或 相关 力 和 关节 运动 之 间 的 关系 ,并 且 可 以 
表示 如 下 : 
M Gn d» F hs (qu Qu) — IP — tn F f (2. 40) 
其 中 ， 
hml amom ) = Cu (CIm sIm ds H Ga (Qu) 
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下 标 mr 用 于 表示 主机 器 人 辟 。 对 于 具有 nn 自由 度 的 串联 和 所 有 关节 旋转 的 机 器 人 机 械 手 ， 
d» «Qn d» EE R" 分 别 是 关节 位 置 、 速 度 和 加 速度 。M (gm) € R”" 表 示 惯 性 矩阵 。hn (gm， 
gm ) 表 示 向 心力 , 科 里 奥 利 力 和 重力 的 非 线性 耦合 项 。 记 表示 库仑 摩擦 ,负载 变化 ,时 间 延 
迟 干扰 和 其 他 干扰 。Jm 是 雅 可 比 矩 阵 ,F 是 人 类 操作 者 施加 的 力 ,rw 是 扭矩 控 制 信号 ,这 两 
者 都 将 被 施加 到 主 装置 。 

3. 从 机 器 人 的 模型 

图 2-14 展示 出 了 Baxter 机 器 人 的 结构 , 它 是 每 臂 具 有 7 自由 度 的 双 臂 机 器 人 。 


机 器 人 的 左 辟 
La 


图 2-14 ”Baxter 机 器 人 的 结 


本 节 将 Baxter 机 器 人 的 仿真 左 辟 作为 从 属 远程 机 器 人 。 标 准 DH 参数 用 于 描述 
Baxter 机 器 人 左 臂 的 结构 ,如 表 2-2 所 示 。 相 关 的 长 度 在 图 2-14 和 表 2-2 中 给 出 。Lw = 
0.27m,La=0.069m,L;=0.364m,L;=0.069m,L=0.375m,L,;s=0.01m,L;=0. 28m, 


表 2-2 从 属 机 器 人 的 DH 参数 (标准 规则 ) 


Link i 6, Cangle limit(deg)) d; a; a; (rad) 
1 qı (一 97.5 一 97.5) Vs Ea —x/2 
2 gt 123~60) 0 0 z/2 
3 4: —7175—175) La La —x/2 
4 qu(2.865~150) 0 0 z/2 
5 qs C 7175. 272—175. 27) La n —n/2 
6 qs 790—120) 0 0 x/2 
7 q; 7115. 27—175. 27) [m 0 0 


为 了 实现 由 主 操纵 杆 命令 的 位 置 的 精确 跟踪 , 主 操纵 杆 和 从 属 远程 机 器 人 之 间 的 工作 
空间 匹配 是 必要 的 。 使 用 的 蒙特 卡 罗 方 法 应 用 于 近似 主 从 的 工作 空间 。 为 了 确保 主 设备 的 
变换 的 工作 空间 被 约束 在 从 属 机 器 人 的 工作 空间 内 , 主 设备 的 工作 空间 以 固定 比例 缩放 。 
图 2-15 表示 出 了 在 匹配 过 程 之 后 主 设备 和 从 设备 的 工作 空间 。 

图 2-15(a) 表 示 出 了 三 维 包 络 表面 , 它 在 匹配 过 程 之 后 作为 主 从 的 工作 空间 的 3D 云 的 
包 络 产生 。Baxter 机 器 人 手臂 和 Touch X 操纵 杆 之 间 的 工作 空间 变换 给 出 为 
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"Og 06-04-02 0 ,02 04 06 08 1 
(b) xy 平面 中 的 工作 空间 匹配 


-0. -0.6 
.8 -0.6 -0.4-0.2 0 02 04 06 08 1 -0.6-04-02 0 02 04 06 08 1 12 
x y 


(e)x-z 平 面 中 的 工作 


; 间 匹 配 (d) y= 平面 中 的 工作 空间 匹配 


图 2-15 工作 区 匹配 


E? cosó -— sind 0 S, 0 01r. T 
y: |= |sinð cos 0|X||0 S，0| | 十 |T， (2.41) 
z, 0 0 1 Q9. 0 S. Eg T. 


HP Crs yss] Dans syn zs 17 分 别 表示 Baxter 和 Touch X 操纵 杆 的 末端 效应 器 的 笛 卡 
儿 坐 标 。$ 是 主 设备 的 基底 的 z 轴 的 旋转 角度 ,[S-,S,,S-]7 和 [T,,T,,T.]" 是 关于 zy 
和 xz 的 比例 系数 和 偏 移 校正 项 轴 。 匹 配 参 数 由 下 式 给 出 : 
S. . 0041) [T. 0, 701 
ge Ss, |= |0.0040| | T, |= |0. 210 (2. 42) 
A 0. 0041] LT, 0. 129 
远程 机 器 人 的 动力 学 模型 可 以 表示 如 下 : 
M; (q: )ğs t+hs (gd)= ts —JiF. f. (2.43) 
h.(q.«q.) = Ci (qi dd; +G, (qs) (2. 44) 
其 中 ,下 标 s 用 于 指示 从 属 。F. 是 环境 和 远程 机 器 人 之 间 的 相互 作用 力 .tc; 是 从 机 的 控制 输入 。 
4. 人 类 操作 员 的 模型 
早期 研究 已 经 表明 ,人 手 的 肌肉 特性 可 以 建 模 为 弹簧 。 在 遥控 操作 系统 中 ,人 手 担 住 主 
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操纵 杆 的 接触 杆 , 并 给 出 相应 的 位 置 和 速度 命令 。 即 使 主 设备 受到 从 从 设备 传输 而 来 的 环 
境 力 ,操作 者 也 可 以 调整 手 的 输出 ,以 使 主 设备 跟踪 操作 者 的 期 望 运动 。 因 此 ,人 手 实 际 上 
可 以 认为 是 由 智能 比例 积分 (PD 控制 器 控制 的 ,智能 比例 积分 (PD 控制 器 可 以 根据 实际 位 
置 zw 和 主 控 器 的 期 望 位 置 zw 之 间 的 误差 来 调整 手 的 输出 力 : 


Fr = Ku Cana — zn)+ Ku | (Emd — Zm )dt (2, 45) 
to 


其 中 ,Ks 和 Ki 分 别 是 人 手 的 比例 增益 和 积分 增益 ,wo 和 1 的 符号 分 别 是 初始 时 刻 和 当前 
时 刻 。 
5. 环境 的 模型 
考虑 环境 的 简单 数学 模型 , 它 描述 了 环境 下 .的 相互 作用 力 与 从 机 器 人 位 置 z, 之 间 的 关 
系 , 即 
F. = Ky Gs — ze)+ Ka d, (2. 46) 
其 中 ,Kw 和 KK 是 环境 的 参数 ,z* 是 环境 的 位 置 。 在 自由 空间 运动 中 ,Kw 一 氏 v 一 0wxvs 


2.2.3 基于 波动 变量 的 神经 控制 设计 


1. 基本 的 PD 控制 设计 
本 节 提 出 了 使 用 基于 标 称 模型 的 转 矩 控制 和 神经 网 络 控制 器 来 处 理 不 确定 性 的 控制 方 
案 。 主 侧 和 从 侧 的 控制 系统 首先 根据 基本 的 PD 控制 技术 设计 。 考 虑 主 设备 和 从 属 机 器 人 
的 动力 学 模型 ,分 别 为 它们 引入 了 以 下 控制 器 : 
rm = Kn em Dn Em (2. 47) 
c, ——K, e, — De (2. 48) 
其 中 ,ei 二 qi 一 qa 是 跟踪 误差 ,qu ER, 是 用 作 局 部 PD 控制 器 的 参考 命令 的 期 望 关 节 角 ,K € 
R, Xn 和 DER,Xn 是 关节 的 对 称 正定 矩阵 角度 和 角速度 增益 。 下 标 “i" 表 示 “m”* 和 “s”, 它 
们 分 别 表 示 主 设备 和 从 机 器 人 。 定 义 广 义 跟踪 误差 为 : 
ew — 6, Ka e, (2. 49) 
对 于 轨迹 跟踪 ,如果 机 器 人 的 动力 学 可 用 ,通过 适当 选择 角 位 置 和 角速度 增益 K RID. 
PD 控制 可 以 保证 闭环 系统 的 稳定 性 。 对 于 不 确定 的 模型 ,传统 的 PD 控制 器 可 能 不 能 保证 
全 局 的 协调 稳定 性 。 如 上 所 述 ,神经 网 络 具 有 强大 的 函数 近似 能 力 , 可 用 于 识别 不 确定 性 。 
人 工 操作 员 的 模型 可 以 被 认为 是 智能 PT 控制 器 , 它 可 以 实时 调整 输出 力 和 位 置 。 本 节 
专注 于 从 属 远程 机 器 人 的 精确 位 置 控制 。 
2. 任务 空间 位 置 到 位 置 控制 
闭环 反 运 动 学 (the Closed Loop Inverse Kinematics,CLIK) 方 法 用 于 从 机 器 人 的 位 置 
控制 , 当 解 决 逆 运 动 学 问题 时 ,可 以 避免 运动 学 奇异 性 和 数值 漂移 。 
所 需 的 从 属 关节 速度 可 以 在 CLIK 算法 中 描述 
qa = | KsJT (geds (2. 50) 
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HEP e= xa 一 Zz; 是 期 望 从 轨迹 zw 和 实际 从 轨迹 之 间 的 误差 ,Ks 是 调整 收敛 速率 的 正定 矩 
阵 ,Jz"(9) 是 雅 可 比 矩阵 的 转 置 。 该 方法 可 以 避免 在 开 环 形式 中 出 现 的 问题 ,并 且 CLIK 算 


法 的 框图 在 图 2-16 中 给 出 。 
1 & 
b 


AU, 


图 2-16 使 用 全 局 神经 控制 器 的 波动 校正 方案 的 远程 操作 控制 


3. RBF 神经 网 络 

RBF 神经 网 络 可 以 利用 其 局 部 泛 化 网 络 的 能 力 去 近似 机 器 人 模型 的 动力 学 , 它 可 以 大 
大 加 快 学 习 速度 ,避免 局 部 最 小 问题 ,提高 机 器 人 的 跟踪 精度 ,特别 是 对 于 结构 复杂 和 大 量 
自由 度 的 机 器 人 ,这 些 优势 更 加 明显 ,RBF 神经 网 络 可 以 表示 为 : 


= 2 
e = exp( I: zz). i = deeem (2.51) 


F(z)= W'gG) (2.52) 
其 中 ,ER, 是 输入 向 量 ,n 表示 从 属 机 器 人 的 自由 度 , 对 于 Baxter 机 器 人 的 左 臂 为 7， 
F(z)ER, 是 输出 向 量 ,WERNXn 是 连接 隐 层 和 输出 层 的 权重 矩阵 , N 表示 隐藏 节点 数 ,ci 
ER, fil, 0 是 中 心 向 量 和 第 i 个 隐藏 节点 的 宽度 。 通 过 径 向 对 称 函数 (例如 ,高 斯 函数 ) 
计算 RBF 神经 网 络 中 的 隐藏 节点 的 输出 。 

RBF 神经 网 络 的 可 调 参 数 是 每 个 隐藏 节点 的 权重 和 矩阵 W, 中 心 向 量 e; 和 宽度 o;。 通 
常 ,c Mo: 的 值 根据 系统 的 知识 或 通过 预 处 理 来 选择 训练 。 网 络 F(z) 的 输出 相对 于 权重 矩 
Vg W 是 线性 的 ,这 极 大 地 简化 了 RBF 神经 网 络 的 分 析 和 学 习 过 程 。 

RBF 神经 网 络 用 于 近似 不 确定 非 线性 函数 F(z) ,并 给 出 以 下 引 理 。 

引 理 1 RBF 神经 网 络 的 输入 向 量 zEX, 其 中 X 是 紧凑 子 集 。 

引 理 2 给 定 正常 数 ee 和 连续 函数 F: zx 一 RR, :存在 权重 矩阵 W* € RN X n 使 得 具有 N 


个 隐藏 层 节点 请 (<) 的 RBF 神经 网 络 的 输出 满足 


max| Paw" )»— FG) [ze (2. 53) 
ec. 
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其 中 ,N dU SE es URP GO Wie. F(z,W) 是 具有 理想 权重 矩阵 Wr 的 输出 F(z) 的 


估计 。 
引 理 3 RBF 神经 网 络 F(z,W) 对 其 参数 <、W 的 输出 是 连续 的 。 因 此 ,可 以 得 到 : 
M. in —— (C, Den + FG W')g (2. 54) 
Jp FG) FGW0,W 是 对 应 于 zEX 的 最 佳 权重 矩阵 , 即 
Ir co- Eos Ww» |= min supll Fco FW) | (2. 55) 
根据 RBF 神经 网 络 的 属性 ,可 以 得 到 : 
M, é,, =— (C, +D, Jen 4-W*"e(z) (2. 56) 
使 用 Lyapunov 方法 很 容易 获得 以 下 更 新 定律 : 
W =— Q glz) ez (2.57) 
其 中 ,Q 是 对 称 正定 矩阵 。 
4. 控制 器 设计 


当 F(z) 关 0mxn 时 , 即 机 器 人 模型 中 存在 不 确定 性 ,并 且 PD 控制 器 可 以 确保 跟踪 误差 的 
有 限 性 ,收敛 到 零 。 因 此 ,RBF 神经 网 络 控制 是 基于 Lemmas 1-3 开发 的 。 控 制 力 矩 由 两 部 


分 组 成 : 
T, = Teb 十 CNN (2.58) 
其 中 ,rep 是 基本 PD 控制 ,并且 根据 上 面 的 推论 ,有 
tro —— K, e, 一 D,e, —— D, e „p (2. 59) 
T cnn 是 神经 网 络 的 补偿 控制 器 
rw = F(z)= Wrp(z) (2. 60) 
然后 从 属 机 器 人 的 闭环 系统 动力 学 可 以 写成 
M,é,, - Coe, +D, e, = F.— Ê, (2.61) 


2.2.4 实验 设计 

1. 主 从 臂 远程 操作 系统 

主 从 臂 远 程 操 作 系统 的 Simulink 框图 如 图 2-17 所 示 , 它 由 Master 模块 .通信 模块 和 
Slave 模块 组 成 。 其 中 ,Master 模块 用 于 接收 Touch. X 的 数据 ,并 进行 初步 处 理 , 输 出 位 置 
误差 ,期 望 速度 和 力矩 。 通 信 模 块 实现 了 波动 变量 功能 ,减少 了 时 延 对 远程 操作 系统 的 影 
响 ,Slave 模块 实现 了 对 Baxter 机 器 人 的 神经 网 络 控制 ,包括 了 Baxter 机 器 人 模型 和 神经 
网 络 控制 算法 的 实现 。 

2. Touch X 的 建 模 

使 用 机 器 人 工具 箱 对 Touch X 进行 运动 学 和 动力 学 建 模 ,其 中 运动 学 建 模 方法 使 用 了 
修改 的 DH 参数 法 ,所 生成 的 Touch X 模型 如 图 2-13 所 示 。 

对 Touch X 建 模 所 使 用 的 代码 如 下 : 
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Joff12 = 0.019; $m horizontal displace between 1,2 joint along negative direction of z 


axis; 

Joff23 - 0.019; $m horizontal displace between 2,3 joint along postive direction of z 
axis; 

L1 = 0.13335; %m length of link2(between joint1,2) = 133. 35mm 

L2 = 0.13335; %m length of link2(between joint1,2) = 133. 35mm 

Lup = 0.02335; %m vertical downward displace 

Lout = 0.16835; %m horizontal outward displace 

clear L; 

deg = pi/180; 

% \Theta d a Nalpha \sigma 


L(1) = Link([0, 0, 0, 0, 0], 'modified'); 
L(1).offset = 0; 
L(1).glim = [- 60 60] * pi/180; * 


L(1).I - [0, 0.35, 0, 0, 0, 0]; 

L(1).r = [0.01783 0.00086 0.19127] + [0,0,0]; * left s0 = joint sO 
L(1).m = 5.70044; 

L(1)m = 200e-6; 

L(1).G = -62.6111; 

L(1).B = 1.48e-3; 

L(1).Tc = [0.395 -0.435]; 

L(2) = Link([O, 0, 0, - pi/2, 0], 'nodified'); 
L(2).offset = 0; 

L(2).dlim = [0 105] * pi/180; & 

L(2).I1 = [0, 0.35, 0, 0, 0, 0]; 

L(2).r = [0.01783 0.00086 0.19127] +[0,0,0]; $ left s0 = joint sO 
L(2).n = 4.70044; 

L(2).Jm - 200e-6; 

L(2).G = 107.815; 

L(2).B = .817e-3; 

L(2).Tc = [0.126 - 0.071]; 


L(3) = Link([ - pi/2, 0, Ll, 0, 0], 'modified'); 
L(3).offset = pi/2; 

L(3).qlim = [ -180 180] * pi/180; * 

L(3).I = [0, 0.35, 0, 0, 0, 0]; 

L(3).r = [0.01783 0.00086 0.19127] * [0,0,0]; % left s0 = joint sO 
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oqn = [0, 0.2689, — 2.209, 0, 0.796, 0.1126]; 
omni 6.plot(oqn); 

view( 100, 45 ); 

clear L; 


3. Touch X 工作 空间 标定 
使 用 MATLAB 机 器 人 工具 箱 对 Touch X 的 工作 空间 进行 标定 ,生成 的 图 形 如 图 2-15 
所 示 , 所 使 用 的 MATLAB 代码 如 下 : 


Clear stl st2 st3 st4 st5 st6 st7a p ; 
N = 10^4*0.6; 


stl = unifrnd( - 60, 60, N, 1) * pi/180; & [- 60 60] 

st2 = unifrnd(0, 105, N, 1) * pi/180; % [0 105] 

% modify to fit the real situation because the range of st3 vary according to the value of st2 
st3 = unifrnd( - 180, 180, N, 1) * pi/180; 各!!! 修改 [一 100 100] --- [ - 180 180] 

st4 = unifrnd(- 145, 145, N, 1) * pi/180; % [- 145 145] 

st5 = unifrnd( -70, 70, N, 1) * pi/180; % [-70 70] 

st6 = unifrnd( - 145, 145, N, 1) * pi/180; % [- 145 145] 


a = size( stl ); 


k = 4; % ratio of mapping the touch x to baxter 


T = omni_6.fkine( [st1(i), st2(i), st3(i) st4(i) st5(i) st6(i) ] ); 
% rotate about the z axis — pi/4 
T = T% trotz( - pi/4); 
% transl(T) 
p(:,i) = transl(T) » 2* [ - 0.005;0.32;0.525]; 
end 


* * [d1,d2] = decision matrix for existence of point (j1, j2, j3) in left and right arm 


% workspace. 

stl = unifrnd( - 1.55, 1.7, N , 1); & [-97.5, 97.5], left_s0; 

%d1 = inpolygon( j1, j2, p(1,k1), p(2, k1)) &&inpolygon(j2, j3, p(2, k2), p( 3, k2) )&&inpolygon( j1, 
j3,p(1,k3), p(3, k3)); 


* d2 = inpolygon (j1, j2, pl (1, kk1), pl (2, kk1)) &&inpolygon ( j2, j3, pl ( 2, kk2), p1 (3, kk2)) 
&&inpolygon(jl, j3, p1(1, kk3), p1(3, kk3)) ; 
st2 = unifrnd( - 1.4, 1.033,N , 1); % [ - 123, 60], left sl 


st3 = unifrnd( - 3.045, 3.05,N , 1); * [—175, 175], left eO 
st4 = unifrnd(-0.05, 2.6, N, 1); % [- 2.865, 150], left e1 
st5 = unifrnd( - 3.05, 3.055, N , 1); 5 [— 175.27, 175.27], left w0 


% joint angle of joint 6 remains constant at 90 deg throughout the 

% experiment 

st6 = unifrnd(- 1.57, 2.09, N, 1); % [-90, 120], left wl 

st] = unitrnd( — 3.055, 3.052, N , 1); 5 [~ 175.27, 175.27], left w2 
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a = size(stl); 


fori=i:1 ia 

T = LeftArm. fkine( [st1(i), st2(i), st3(i) st4(i) st5(i) 1.57 st7(i)] ); 
p2(:,i)- (transl(T) - [0.63;0.825;0.0365]) * 8/100; 

p2(:,i) = transl(T); 

end 


% % Plotting the workspaces of both the arms with convhull 
figure; 
plot3( p2(1, :), p2(2, :), p2(3, :), 'r.'); 
hold on; 
DT = delaunayTriangulation(p'); 
[K2,v] = convexHull(DT); 
trisurf(K2,DT. Points(:, 1), DT. Points( :,2), DT. Points(:,3), ... 
'FaceColor', 'r') 
plot3( p(1, :), p(2, :), p(3,:), 'b.'); 
DT1 = delaunayTriangulation(p2'); 
[K2,v] = convexHull(DT1); 
trisurf(K2,DT1.Points(:,1),DTl. Points(:,2), DT1. Points(:,3), ... 
'FaceColor', 'b') 
xlabel('X', 'FontSize',10, 'FontWeight', 'bold', 'Color', 'k') 
ylabel('Y','FontSize',10, 'FontWeight', 'bold', 'Color', 'k') 
zlabel('Z', 'FontSize',10, 'FontWeight', 'bold', 'Color', 'k') 
hold on 
grid on; 
box off; 
view(45, 45); 
* % Plotting the xy 
figure; 
plot(p(1,:), p(2, :), 'b.'); 
hold on; 
pilot(p2(1, :), p2(2, =), es 
xlabel('X', 'FontSize',10, 'FontWeight', 'bold', 'Color', 'k') 
ylabel('Y', 'FontSize',10, 'FontWeight', 'bold', 'Color', 'k') 
box off; 
% % Plotting the yz 
figure; 
plot( p(2, :), p(3, :), 'b.'); 
hold on; 
plot( p2(2, :), p2(3, :), 'r.'); 
xlabel('Y', 'FontSize',10, 'FontWeight', 'bold', 'Color', 'k') 
ylabel('Z','FontSize',10, 'FontWeight', 'bold', 'Color', 'k') 
box off; 
* % Plotting the xz 
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figure; 

plot( p(1,:), p(3, :), 'b.'); 

hold on; 

Blot( palt: J; pol3, =); 7 

xlabel( 'X', 'FontSize',10, 'FontWeight', 'bold', 'Color', 'k') 
ylabel('Z', 'FontSize',10, 'FontWeight', 'bold', 'Color', 'k') 
box off; 


2.2.5 仿真 实验 


为 了 以 下 目的 进行 比较 实验 : 

(1) 证 明 与 无 补偿 的 典型 PD 控制 相 比 ,RBF 神经 网 络 补偿 有 效 地 改善 跟踪 性 能 。 

(2) 证 明 波 可 变 技 术 增 强 的 主 -从 远程 操作 系统 在 存在 各 种 时 间 延 迟 的 情况 下 保持 

(3) 说 明 遥 操作 系统 的 神经 控制 方案 和 波动 变量 技术 的 无 颖 组 合 。 

1. 实验 平台 

实验 平台 设置 有 Touch X 触觉 设备 和 连接 到 它 的 计算 机 ,以 及 使 用 MATLAB 机 
器 人 工具 箱 仿真 的 从 机 器 人 Baxter( 见 图 2-18) 。 人 类 操作 者 移动 Touch X 操纵 杆 的 接 
和 触 杆 ,通过 该 接触 杆 将 期 望 的 轨迹 发 送 到 模拟 的 通信 信道 ,并 且 调 节 模 拟 的 机 器 人 和 
环境 之 间 的 接触 力 。 通 过 通信 通道 ,从 机 器 人 的 期 望 位 置 被 传递 到 Baxter 机 器 人 的 仿 
真 左 臂 。 


LeftAm. 

0.5 1 

N 0 0.5 b 
-0.5 * 9 
E -0.5 
1.5 > 3t 

"T 、 2» 
TEM —— 0 -1.5 
15 105 0 -3 E ul. 1 0 4 
oce z Y 


E 2-18 仿真 的 从 机 器 人 


2. 不 同 控制 器 下 的 轨迹 跟踪 

首先 ,将 从 机 器 人 的 控制 器 设计 为 典型 的 PD 控制 器 ,并 设计 了 由 主机 设备 (Touch X) 
为 从 机 器 人 (仿真 的 Baxter 机 器 人 臂 ) 设 置 的 期 望 轨 迹 , 其 要 求 操作 者 将 主 操纵 杆 的 接触 
杆 从 初始 位 置 沿 着 工 方向 移动 到 最 小 值 :然后 沿 x 方向 达到 最 大 值 .最 后 恢复 到 初始 位 
置 , 时 间 跨 度 为 0 一 3s。 然 后 ,人 类 操作 者 沿 着 y 方 向 执行 相同 的 运动 3 一 6s, 并 且 沿 着 = 
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方向 执行 相同 的 运动 6 一 9s。 具 有 传统 PD 控制 器 的 从 机 器 人 的 轨迹 跟踪 结果 如 图 2-19 


所 示 。 
B ior 
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Bio 
mge 200 400 600 800 1000 
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Rio ' i i : 
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E 20r 
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图 2-19 传统 PD 控制 器 的 主 设备 ( 实 线 ) 和 从 设备 (虚线 ) 的 轨迹 


然后 ,在 基本 PD 控制 器 之 上 添加 RBF 神经 控制 器 ,以 将 不 确定 的 非 线 性 动力 学 补偿 
在 该 实验 中 ,保留 前 10s 用 于 神经 网 络 训练 。 

显然 ,不 同 关 节 的 权重 收敛 到 不 同 的 值 ,其 中 关节 S1. Wo 和 W2 的 标准 权重 的 值 接 近 
于 0, 因 为 它们 在 机 器 人 的 移动 期 间 几 乎 不 受 重力 影响 。 在 10 一 19s, 人 工 操作 者 重复 最 后 
一 个 实验 的 过 程 。 神 经 网 络 的 权重 如 图 2-20 所 示 。 与 如 图 2-19 和 图 2-21 所 示 的 两 个 实验 
的 跟踪 性 能 相 比 ,在 添加 RBF 神经 网 络 控制 器 之 后 ,从 机 器 人 的 跟踪 性 能 大 大 提高 。 


“0 200 400 600 800 1000 1200 1400 1600 1800 2000 
tims 


图 2-20 神经 网 络 训练 期 间 从 机 器 人 的 每 个 关节 的 神经 网 络 权重 的 标准 
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图 2-21 使 用 RBF 神经 网 络 补 偿 的 主 设备 ( 实 线 ) 和 从 设备 (虚线 ) 的 轨迹 


3. 在 不 同 的 通信 下 的 轨迹 和 力 反 馈 

在 时 变 延 迟 的 情况 下 ,系统 很 可 能 变 得 不 稳定 和 不 可 控制 。 接 下 来 测试 远程 操作 系统 
通信 中 的 波动 变量 技术 ,然后 在 不 使 用 波动 变量 的 情况 下 与 其 他 实验 组 进行 比较 。 实 验 中 
添加 的 时 间 延 迟 T, 和 Ts 如 图 2-22 所 示 。 图 2-22 中 的 力 反 射 到 人 类 操作 者 。 图 2-23 是 通 
过 Fn 一 Ke(zs 一 zn) 计 算 的 。 
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图 2-22 变化 时 间 延 迟 Ti( 实 线 ) 和 T: CHE AO 
人 类 操作 者 需要 重复 上 一 次 实验 的 两 次 移动 ,一 次 在 通信 信道 中 具有 波 变 量 , 另 一 次 没 
有 。 两 个 比较 实验 的 轨迹 跟踪 性 能 如 图 2-23 和 图 2-24 所 示 。 在 力 反 射 实验 中 ,类 似 于 壁 
的 刚性 工件 被 建立 并 沿 z 方 向 安装 ,如 图 2-23 所 示 。 在 前 3s 中 , 主 设备 和 从 属 机 器 人 都 处 
于 自由 运动 。 然 后 , 主 件 开始 朝向 从 件 将 接触 工件 的 位 置 移动 ,并 且 接 触 将 持续 约 10s。 在 
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与 工件 接触 期 间 , 从 机 器 人 几乎 不 移动 ,施加 在 从 机 器 人 上 的 环境 力 收敛 到 设 定 值 ,并 且 操 
作者 以 恒定 的 力 保持 主 装置 。 然 后 ,操作 员 将 主 操纵 杆 移动 回 自 由 运动 ,并 且 从 动机 器 人 离 
开工 件 并 跟踪 主动 作 而 没有 时 变 延 迟 。 图 2-25 表示 了 沿 着 主 设备 和 从 属 远程 机 的 工 方向 
的 轨迹 以 及 具有 时 变 延 迟 和 波 变 量 的 力 反射 , 图 中 的 振荡 是 由 于 在 接触 之 前 和 之 后 的 力 反 
馈 的 过 程 引起 的 。 


图 2-23 使 用 波 可 变 技术 的 主 设备 ( 实 线 ) 和 从 设备 (虚线 ) 轨 迹 随 时 变 延 迟 通 信 
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图 2-24 不 使 用 波 可 变 技术 的 主 设备 ( 实 线 ) 和 从 设备 (虚线 ) 轨 迹 随时 变 延 迟 通信 
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图 2-25 (顶部 ) 使 用 波 可 变 技 术 , 随 着 时 间 延 迟 通信 , 沿 着 主 设备 ( 实 线 ) 和 从 设备 (虚线 ) 的 x 方向 
的 轨迹 ; (中) 对 从 机 器 人 的 环境 力 ; (底部) 使 用 波 可 变 技 术 , 随 着 时 变 延 迟 通信 , 沿 着 x 
方向 对 主 设备 施加 反馈 


2.3 开发 混合 运动 捕捉 方法 使 用 MYO 手 环 应 用 于 远程 
操作 


本 节 介 绍 根据 2. 2 节 所 述 方法 搭建 的 仿真 系统 的 设计 方法 ,编程 与 实现 。 
2.3.1 引言 


1. MYO EX 

如 图 2-26 所 示 ,MYO 手 环 (手势 控制 臂 环 ) 是 加 拿 大 创业 公司 Thalmic Labs 推出 的 创 
新 性 臂 环 ,佩戴 者 只 要 动 动手 指 或 者 手 , 就 能 操作 科技 产品 ,与 之 发 生 互动 。 手 势 控制 臂 环 
可 以 佩戴 在 任何 一 条 有 骆 膊 的 前 臂 ,探测 用 户 的 肌肉 产生 的 电 活动 。 它 通过 低 功率 的 蓝牙 设 
备 与 其 他 电子 产品 进行 无 线 连接 ,不 需要 借助 相机 就 可 感知 用 户 的 动作 。MYO 手 环 由 9 
轴 IMU 测量 单元 (三 轴 陀 螺 仪 三 轴 加 速度 .三 轴 磁 场 ) 和 8 个 肌 电信 号 传感器 组 成 。 通 过 
9 轴 惯 性 测量 单元 可 以 有 效 地 测量 穿戴 者 的 手臂 姿态 ,而 通过 8 个 肌 电信 号 传感器 可 以 检 
测 操作 者 手臂 皮肤 表面 的 肌 电 信号 ,用 于 识别 操作 者 的 手势 。 通 过 MYO 手 环 提供 的 肌 电 
信和 号 可 以 有 效 地 估计 操作 者 的 手势 。MYO 手 环 测量 的 数据 可 以 通过 蓝牙 传输 到 计算 机 。 

2. 人 类 关节 运动 学 模型 

健康 的 人 类 手臂 包括 肩 关 节 、 肘 关节 和 腕 关节 ,由 7 个 自由 度 组 成 ,其 中 肩 关节 包括 
3 个 自由 度 , 肘 关节 包括 2 个 自由 度 , 腕 关节 包括 2 个 自由 度 。 

本 节 使 用 标准 DH 参数 ( 见 表 2-3) 的 方法 ,建立 了 代表 人 体 手臂 的 运动 学 模型 ,如 
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图 2-27 所 示 。 这 里 假设 人 类 手臂 是 一 个 7 自由 度 串 联机 器 人 手臂 , 表 2-3 显示 相关 的 DH 
参数 。2 轴 表 示 旋 转轴 。L: 表示 操作 者 上 臂 的 长 度 。L: 表示 操作 者 的 下 臂 长 度 。Ls 是 手 
掌 的 长 度 。L1、L: RI Ls 的 单位 为 米 。 假 设 手腕 偏 航 角 为 零 。 


图 2-26 MYO F 图 2-27 人 类 肢体 的 运动 学 模型 


R23 人 体 动力 学 模型 的 标准 DH 参数 


i [71 d. ài ai 

i qı 0 0 —90* 
2 q +90° 0 0 90* 
3 gs E 0 —90* 
4 qi 0 0 90° 
5 qs L: 0 —90* 
6 qs 0 0 90° 
7 qr L 0 0 


2.3.2 设计 方法 


1. 肩 部 和 肘 部 运动 捕获 方法 I 

如 图 2-28 所 示 , 全 局 坐标 系 (X6, Yc, Zo) 
定义 为 : X 轴 指向 侧面 ; Y 轴 向 前 ,Z 轴 向 上 。 
假设 最 初 肪 骨 参 考 坐 标 系 (Xr,Yr,Zr) 和 前 臂 
参考 坐标 系 (Xr,Yrf,Zr) 与 全 局 坐标 系 重 合 , 因 
此 能 够 确定 旋转 矩阵 Ror = [Xsr Yor Ze] 
和 Rin = [Xen Yan Zénj, 其 中 R 表 示 旋 转 
矩阵 ;上 标 “i” 表 示 初 始 位 置 ;下 标 “"G”、“H” 和 
“F” 分 别 表示 全 局 参考 坐标 系 、 肽 骨 参 考 坐 标 系 
Tn E HR ERR. MESSA DE Rww 表 示 参 考 坐 图 2-28 全 局 参考 坐标 系 以 及 局 部 参考 坐标 系 
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WR N ”相对 于 参考 坐标 系 "M” 的 姿态 。 列 向 量 Xu Y ws ,Zuw 表 示 用 参考 坐标 系 *M "表示 
坐标 系 “N” 的 坐标 轴 方 向 的 单位 向 量 ,Xwn Ys Zu € RP. 

大 骨 坐 标 系 相 对 于 第 一 个 MYO 臂 环 坐标 系 和 下 臂 坐标 系 相 对 于 第 二 个 MYO 臂 环 坐 
标 系 的 取向 分 别 表 示 如 下 : 

Rin = (Ra) Rio. Rir = (ORG)! Roe (2. 62) 

其 中 ,下 标 U 表示 佩戴 在 上 臂 上 的 第 一 MYO 手 环 坐标 系 , 下 标 工 表示 佩戴 在 下 臂 上 的 第 
二 MYO 手 环 坐标 系 。 

当 操作 者 将 手臂 移动 到 新 的 姿势 时 ,全 局 坐标 系 中 的 肽 骨 和 前 臂 的 取向 可 以 由 以 下 旋 
转 矩阵 描述 ， 


Réu = RéuRin, RE = R&Rir (2.63) 
其 中 ,上 标 了 表示 操作 者 的 手臂 新 姿势 。 
从 第 一 个 MYO 的 陀螺 仪 ,获得 四 元 数 q = [xz,y,z'w] ,其 中 (x,y,z) 是 向 量 ,w 是 
标量 。 
q—xctytudtw (2. 64) 
从 四 元 数 中 JUPE E A en AS b IR n S REEL ULT : 
Leti riz ris 
Rén = f r2 Ta 


muo fe Ta 


1— 2f +z) Z(ry — wz) 2(wy + az) 
2(zy T wz) 1—2(z* 4- 2?) 2C(yz — wr) 
2(xzz — wy) 2(wr + yz) 1—2G* +y’) 

AM IE Ale bs R 9 28 f HE Fo ILES 2-29。 考 虑 参考 坐标 系 {B} 和 坐标 系 {A} 之 间 的 
3 个 欧 拉 角 , 即 y,B8,a, 它 们 分 别 是 回转 角 、 俯 仰角 和 偏转 角 。 首 先 , 参 考 坐标 系 { 了 } 与 参考 
坐标 系 {A) 重 合 ,然后 参考 坐标 系 {B} 绕 x 轴 旋 转 y 弧度 ,然后 绕 y 轴 旋 转 p 弧度 ,最 后 绕 < 
轴 旋 转 a 弧度 。 因 此 ,可 以 写成 如 下 形式 : 

Rtu = Rz(a) Ry(B) Rx Cp 

Cach  caspsy — sacy casBcy + sasy 
sacg sasßsy 十 cacy sasßcy — casy 
-=g — dw chey 
其 中 ,cB 是 cosp 的 简写 ,8 是 sing 的 简写 。 

从 两 个 MYO 手 环 的 读数 ,可 以 计算 操作 者 手臂 的 前 五 个 关节 角度 。 从 上 面 的 式 子 可 
以 计算 3 个 肩 关节 角度 ( 肩 部 弯曲 /伸展 、 外 展 /内 收 和 内 /外 旋转 ): 


(2. 65) 


(2. 66) 


B= Atan2C Vra + r32 «ra? (2. 67) 
a = AtanZ(rz/sB eri /s8) (2. 68) 


y = Atan2Z(rg/sB8.— ra /s9) (2. 69) 
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2-29 人 体 腹 膊 坐标 系 的 关节 角度 图 示 


HEP ya 和 8B 分 别 代 表 肩 关节 回转 角 、 偏 转角 和 俯仰 角 。 
通过 从 两 个 MYO 手 环 收集 的 数据 ,还 可 以 计算 两 个 肘 关节 角度 ( 肘 届 曲 /伸展 和 旋 前 / 
旋 后 ) ,通过 第 二 个 MYO 臂 环 可 以 得 到 如 下 旋转 矩阵 : 
an di di 
Rá. = [e az M (2, 70) 
31 ds2 as. 
因此 可 得 : 
Ar = arccos(aizr13 + azzrzs + azra) (2.71) 
Ap = arccos(an ri Haa ra às ra) (3. 72) 
其 中 ,Ar 和 Au 分 别 表 示 肘 关节 屈曲 和 肘 弯 的 关节 角度 。 
2. 肩 部 和 肘 部 运动 捕获 方法 耳 
为 了 估计 上 臂 的 旋转 角度 ,需要 先 找 出 用 于 描述 操作 者 上 臂 三 个 旋转 轴线 的 旋转 矩阵 。 
Wre „Waa 、WF 是 相对 于 胸部 坐标 系 的 旋转 矩阵 ,分 别 用 来 描述 上 臂 届 曲 伸展 .外 展 内 收 和 
内 旋 外 旋 的 旋转 轴 。 
计算 旋转 和 矩阵 Wre 的 步骤 如 下 : 
D 操作 者 手臂 戴 两 个 MYO 手 环 站 立 , 将 手 辟 自然 放 在 一 侧 。 
© 让 操作 者 以 缓慢 速度 完全 弯曲 上 辟 , 到 达 手 辟 弯 曲 上 限 后 ,将 手臂 伸 回 到 初始 位 置 。 
计算 旋转 矩阵 WA 的 步骤 如 下 : 
(D 让 操作 者 以 缓慢 的 速度 完全 外 展 上 臂 。 
Q 让 操作 者 将 手臂 加 回 到 初始 位 置 。 
通过 相同 的 过 程 , 以 下 面 的 方式 找到 We。 让 操作 者 的 手臂 以 适当 的 速度 向 内 充分 旋 
转 ,然后 旋转 回 到 初始 位 置 。 操 作者 每 个 动作 执行 5 次 。 为 了 找到 胸部 坐标 系 中 上 辟 届 曲 
伸展 的 旋转 轴 ,假设 胸部 坐标 系 是 固定 的 ,并 且 胸 部 的 坐标 系 与 全 局 坐标 系 重 合 。 上 臂 的 
MYO 手 在 最 初 参考 坐标 系 中 的 姿态 用 旋转 矩阵 表示 如 下 : 
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RË = (QU) Ri (2. 73) 
RÖ 的 旋转 轴 可 以 由 下 面 的 对 称 和 矩阵 表 示 : 
0 一 也 Wy 
WE = RY — (RẸ )" = | w: 0 = (2. 74) 
— Wy Wr 0 
旋转 矩阵 外 区 关于 旋转 轴 的 描述 为 ， 
W = WE/ Jw w;d wes (2. 75) 
因为 胸部 坐标 系 与 全 局 坐标 系 重合 ,所 以 胸部 坐标 系 中 的 旋转 轴 的 旋转 矩阵 可 以 写 为 
Wre = RGW (2.76) 


T5] 8E ET AGI AXE T TR PBE P Jie P] Je (6) WEE 8 AO EXTERE e. PT DUET Hi E 
坐标 系 在 胸部 坐标 系 中 的 姿态 为 
Rn = (Rér) "Rén (2.77) 
其 中 ,下 标 “T” 表 示 胸 部 坐标 系 。 在 时 间 步 又 ,及 骨 坐标 系 相 对 于 初始 姿态 的 的 旋转 矩阵 
可 以 计算 为 
Ri" = (Rm Rir (2.78) 
刚体 围绕 固定 轴线 旋转 以 指数 形式 描述 为 R — e" ,其 中 qER 是 围绕 旋转 轴线 的 旋转 
角度 ,用 更 明确 的 形式 可 以 表示 为 


R= e^ = 1+ Wsing +W C — cosg) (2.79) 
Hp w= Go, cw, w) € RW 表示 刚体 的 旋转 轴 。 在 肩 关 节 中 有 3 个 自由 度 ,使 得 上 
臂 的 运动 可 以 分 解 成 3 个 连续 的 旋转 ,分别 是 围绕 屈伸 伸展 的 轴线 .外 展 内 收 的 轴线 和 内 旋 
外 旋 的 轴线 ,计算 公式 如 下 
及 souuer = e e e em e" ^n (2. 80) 
其 中 ,Raouaee 和 式 (2.78) 中 的 R2 表 示 上 臂 相 同 的 运动 。grz qaa .dB 分 别 是 相对 于 伸展 的 轴 
线 、. 外 展 内 收 的 轴线 和 内 旋 外 旋 的 轴线 的 旋转 角 。 
下 臂 的 最 终 位 置 相对 于 第 二 个 MYO 手 环 坐标 中 的 初始 位 置 的 姿态 是 


RY = (Ra) 'R& (2. 81) 
RY 的 旋转 轴 用 斜 对 称 和 矩阵 RF 表示 为 
WE = RY — (Rf )" (2. 82) 
下 避 MYO 坐标 系 旋 转轴 用 旋转 矩阵 表示 为 
W: = WE / wi, + wi, + wi. (2. 83) 
其 中 ,2. w, wi EWE WIRK. HTE MYO 手 环 坐 标 系 中 ,旋转 轴 的 姿态 表示 为 
Wi = Ri W: (2. 84) 
Jer ^e bs ZR rbi a 0 8s 
We = (Ri ) Ra Wi (2. 85) 


WEE Abs R TES TE Ae bs R P fO 
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Rir = (Rén ) Ree (2. 86) 
FREE Hg 2 rh D Es 
Rg” = (Rue) Rhe (2. 87) 
考虑 到 肘 部 有 两 个 自由 度 。 肘 的 运动 可 以 由 两 个 连续 的 旋转 组 成 : 一 个 是 关于 肘 关节 
的 届 曲 伸展 , 另 一 个 是 关于 肘 关节 的 内 旋 外 旋 
Rae. = eU tm ew ors (2. 88) 
其 中 ,Raww 和 RY 是 描述 操作 者 肘 关 节 运 动 的 旋转 和 矩阵 ,gre 和 g ps 分别 是 相对 于 轴 关 节 届 曲 
延伸 轴 和 内 旋 外 旋 轴 的 旋转 角度 。 
3. 手腕 关节 角度 估计 
当 MYO 手 环 佩戴 在 操作 者 的 前 臂 上 时 , 它 能 够 通过 EMG 信号 识别 操作 者 的 手 姿势 。 
通过 确定 的 手势 ,基于 EMG 信号 建立 关节 角度 模型 。 这 里 只 估算 手腕 的 回转 角 。 令 0 表 
示 腕 关节 角 , 其 中 ;=1,2 表示 不 同 的 旋转 方向 。EMG(IEMG) 是 一 个 引入 的 变量 ,可 以 通 
过 应 用 全 智能 整流 和 平滑 得 到 。EMG 信号 的 幅度 作为 IEMG 信和 号 的 平均 值 的 AIEMG 特 
征 表示 。 其 中 ,IEMG1(n) (n 二 1,… .8) 是 第 1 个 EMG 传感器 测量 第 n 参考 坐标 系 中 的 
IEMG 信号 的 值 。 第 n 参考 坐标 系 中 的 AIEMG 特征 的 平均 值 计算 如 下 


AIEM G, (m) = T [ iE Gà (2. 89) 
假设 腕 关节 角度 的 幅度 和 ENG 信和 号 之 间 的 关系 是 近似 线性 的 。 腕 关节 角度 计算 为 


EG È 3AIEMG, Q0) (2, 90) 


Kp onm 是 手势 i 中 角度 的 最 大 值 社 一 1 表示 手掌 向 内 ,i 二 2 表示 手掌 向 外 。E?* 和 Er?™ 
分 别 是 EE(n) 的 最 大 值 和 最 小 值 ,因此 


à, = E(n)— E?^ 
te BEP—EP 


2.3.3 仿真 系统 设计 


根据 编者 前 期 研究 工作 ,下 面 介 绍 根据 上 述 方 法 搭建 的 仿真 系统 的 编程 与 实现 。 设 计 
仿真 系统 如 图 2-30 所 示 。 它 的 工作 原理 为 : MYO 手 环 用 于 采集 操作 者 手臂 的 姿态 数据 ， 
经 过 无 线 蓝牙 发 送 到 计算 机 。 然 后 通过 一 个 C++ 程序 对 这 些 数据 进行 处 理 , 计 算出 操作 者 
手臂 的 关节 角度 ,包括 3 个 肩 关节 角度 和 2 个 肘 关节 角度 ,从 而 确定 手臂 的 姿态 。 最 后 , 通 
过 无 线 网 络 将 关节 角 发 送 到 另 一 台 计 算 机 中 的 MATLAB 程序 ,用 于 控制 虚拟 的 机 器 人 


模型 。 
蓝牙 = | 
操作 者 “| 一 一 >| 数据 处 理 一 >| HH 
(MYO TH) (C++ 程序 ) (MATLAB) 


图 2-30 仿真 系统 的 结构 


or (2.91) 


第 2 章 ”MATLAB 机 器 人 工具 箱 的 应 用 


1. 使 用 程序 接收 并 处 理 MYO 手 环 传输 的 数据 
所 使 用 的 代码 如 下 : 
if (identifyMyo(myo) == 1) 


{ 
//Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion. 


all = 1 - 2 x (quat.y()* quat.y() + quat.z() * quat.z()); 
al2 = 2 * (quat.x() * quat. y() — quat.z() * quat.w()); 
al3 = 2 * (quat.x() *quat.z() + quat. y() x quat. w()); 
a21 = 2 * (quat.x() * quat.y() + quat.z() *»quat.w()); 
a22 = 1 - 2 x (quat.x() *quat.x() + quat.z() * quat.z()); 
a23 = 2 * (quat.y() * quat.z() - quat.x() x quat. w()); 
a31 = 2 * (quat.x() *quat.z() - quat.y() x quat. w()); 
a32 = 2 * (quat.y() * quat.z() + quat.x() * quat.w()); 
a33 = 1 - 2 * (quat.x() * quat.x() + quat.y() x quat. y()); 


/ /calculated the three shoulder joint angles. 

float pitch = atan2(- 1 * a31, sqrt(all* all + a21*a21)); 
float yaw = atan2(a21 / (cos(pitch)), all / (cos(pitch))); 
float roll = atan2(a32 / (cos(pitch)), a33 / (cos(pitch))); 


// 转 换 为 度数 
pitch w = pitch / 3.14 x 180; 
roll w= -1% roll/3.14 * 180; 


yaw w = yaw/ 3.14 * 180; 
) 
else 


$ 
//Calculate Euler angles (roll, pitch, and yaw) from the unit quaternion. 


bll = 1 - 2 * (quat.y() * quat.y() + quat.z() * quat.z()); 
bl2 = 2 * (quat.x() * quat.y() — quat.z() » quat.w()); 
bl3 = 2 * (quat.x() * quat.z() + quat.y() * quat.w()); 
b21 = 2 * (quat.x() *quat.y() + quat.z() x quat.w()); 
b22 = 1 - 2 * (quat.x() * quat.x() + quat.z() x quat.z()); 
b23 = 2 * (quat.y() * quat.z() — quat.x() * quat.w()); 
b31 = 2 * (quat.x() *quat.z() — quat.y() * quat. w()); 
b32 = 2 * (quat.y() * quat.z() + quat.x() * quat.w()); 
b33 = 1 - 2 * (quat.x() * quat.x() + quat.y() * quat. y()); 


//calculated the elbow joint angles 

float Pitch = acos(all*b1ll + a21*b21 + a31 * b31); 

//float Roll = acos((a31* b21 — a21* b31) * bl2 + (all*b31 - a31*bll) x b22 + (a21«* 
bll — all* b21) x b32); 

float Roll = acos(al2 * bl2 + a22 * b22 + a32 * b32); 

// 转 换 成 度数 

Pitch w = Pitch/3.14 x 180; 

Roll w = Roll/3.14 * 180; 
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2. 使 用 MATLAB 机 器 人 工具 箱 对 机 器 人 进行 建 模 
所 使 用 的 MATLAB 代码 如 下 : 


省 设置 机 器 人 的 连 杆 长 度 
0 
12 = 0.069; 
13-0.28; 
deg = pi/180; 
clear L; 
* 使 用 标准 的 DB ERATA k FR EITE 
* Ntheta d a Nalpha Vsigma 
$0.1 50.069 
L(1) = Link([O, 0.1, 0.069, -pi/2, 0], 'standard'); 
L(2) = Link([pi/2, 0, O0, pi/2, 0], 'standard'); 
L(3) = Link([O, 0.364, I2, -pi/2, 0], 'standard'); 
L(4) = Link([0, 0, 0, pi/2, 0], 'standard' ); 
L(5) = Link([0, ^ 0.104*0.271, 0.01, -pi/2, 0], 'standard'); 
L(6) = Link([O, 0, O0, pi/2, 0], 'standard'); 
L(7) = Link([0, ^ 0.28, 0, 0, 0], 'standard'); 
5 显示 手臂 的 名 称 
D H = SerialLink(L, 'name', 'robot arm'); 
5 设置 初始 位 姿 


oqz= [0,0,0,0,0,0,0]; 
oqn = [0, pi/2,0,0,0,0,0]; 
当面 出 机 器 人 模型 

D H. plot(oqn); 

设置 观察 角度 
view(100,45); 

clear L; 


2.3.4 仿真 实验 


为 了 验证 仿真 系统 的 有 效 性 ,开展 了 以 下 实验 : 让 一 个 操作 者 参与 到 实验 中 。 操 作者 
在 同一 个 手臂 上 穿戴 两 个 MYO 手 环 ,保持 站 立 的 姿势 。 第 一 个 MYO 手 环 佩戴 在 上 臂 中 
心 附近 ,第 二 个 MYO 手 环 佩戴 在 前 臂 中 心 附近 。 利 用 本 书 介绍 的 MATLAB 机 器 人 工具 


箱 建立 虚拟 的 机 器 人 手臂 ,其 初始 姿势 如 图 2-31 所 示 。 


在 操作 者 控制 虚拟 机 器 人 臂 之 前 ,MYO 手 环 必须 被 校准 ,并 且 EMG 传感器 必须 进行 
一 段 时 间 的 “适应 ”操作 者 手臂 ,使 得 MYO 手 环 能 够 辨别 不 同 的 手 姿势 。 操 作者 不 能 移动 


位 置 ,使 得 只 有 两 个 臂 可 以 自由 移动 。 而 且 , 实 验 过 程 中 ,了 


臂 的 移动 速度 不 能 过 快 。 


如 图 2-32 所 示 ,可 以 看 到 , 当 操 作者 以 适当 的 速度 移动 臂 时 ,虚拟 机 器 人 臂 很 好 地 跟随 
操作 者 的 手臂 运动 。 操 作者 手臂 在 不 同 姿势 下 的 方向 如 表 2-4 和 表 2-5 Bron ,前 三 行 的 值 
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2-31 机 器 人 模型 的 初始 位 姿 


分 别 用 于 俯仰 角 、 回 转角 和 偏转 角 。 第 四 和 第 五 行 的 值 为 肘 关节 的 弯曲 和 回转 角度 ,第 六 行 
为 手腕 关节 角度 。 可 以 看 到 ,两 种 方法 都 能 够 捕获 人 的 手臂 姿势 。 
表 2-4 第 一 种 方法 计算 不 同位 置 的 操作 者 筑 的 关节 角度 


姿 态 俯仰 角 BRA 1 偏 航 角 Tib fü WR | 手腕 翻 深 角 
1 12. 58 —17.44 12. 36 18.72 5.70 0 
2 4. 41 一 31.54 一 11.78 12.85 19.31 42. 85 
3 一 15.42 一 28.08 62.70 18. 59 12. 34 0 
4 —15.61 —19.54 53. 28 6.5 47. 69 0 
5 —64.96 —18.30 20. 04 16. 63 47. 69 0 
6 6. 63 一 21.30 一 8.79 89.58 17. 54 0 
7 0.07 —22.65 — 10. 28 73.02 27.49 0 
8 一 34.87 一 58.39 22. 93 65. 68 30.05 75. 08 


RLS 第 二 种 方法 计算 不 同位 置 的 操作 者 臂 的 关节 角度 


姿 态 俯仰 角 翻滚 角 1 偏 航 角 屈伸 角 翻滚 角 2 | 手腕 翻滚 角 
1 18. 00 一 8. 02 一 4. 67 16. 33 0.78 0 
2 12.18 —24.52 —6. 23 7.92 21.34 42. 85 
3 —21.34 —40.41 47. 55 8.63 13. 51 0 
4 —4.92 —86.27 41. 59 36.6 12. 60 0 
5 —11.18 —21.12 —31.01 14.28 5.56 0 
6 —4. 19 —21.52 — 12.36 81.25 21.84 0 
7 5.17 —19.51 —8.61 71.00 4.16 0 
8 —42.61 —66.68 —59. 90 60.19 27. 81 75.08 
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图 2-32 操作 者 的 不 同 手臂 姿态 控制 不 同 的 机 械 臂 姿态 (使 用 MATLAB 工具 箱 中 的 仿真 机 器 人 ) 
(a) 姿态 1. 操作 者 的 手臂 初始 姿势 ; (b) 姿态 2: 操作 者 的 手 波 入 ;(c) 姿态 3: 操作 者 的 肩膀 内 收 ; (d) 姿态 4; 
操作 者 的 肩 部 内 旋 ; CO 姿态 5: 操作 者 的 手臂 放下 ; (O 姿态 6: 操作 者 的 肘 伸展 ; (g) 姿态 7: 操作 者 的 肘 关 节 ; 
(h) 姿态 8: 操作 者 的 复杂 姿势 


2.4 基于 自 适 应 参数 识别 的 Geomagic Touch X 触觉 装置 
运动 学 建 模 


本 节 主 要 介绍 基于 自 适 应 参数 识别 的 Geomagic Touch X 触觉 装置 运动 学 建 模 的 步 
了 又、 仿真 设计 、 编 程 与 实现 。 
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2.4.1 引言 


在 过 去 二 十 年 中 , 力 反 馈 设备 的 商业 化 已 使 得 触觉 技术 广泛 应 用 在 各 个 方面 ,例如 虚拟 
现实 、 遥 操作 ,娱乐 和 医学 研究 。 触 觉 技 术 在 如 牙 修复 和 医学 模拟 中 成 为 研究 热点 ,引起 了 
广泛 的 关注 。 

Geomagic Touch X 是 由 SensAble 技术 公司 开发 的 触觉 装置 ,不 仅仅 提供 了 力 反 馈 装 
置 ,还 提供 了 硬件 驱动 程序 和 软件 包 (OpenHaptics 工具 包 )。 它 支持 一 系列 如 手动 旋转 的 
动作 ,支持 各 种 商业 、 学 术 和 研究 应 用 。 该 装置 是 专门 为 动 觉 反馈 设计 ,包含 了 一 系列 机 器 
手臂 以 及 一 个 手柄 。 它 的 机 器 手臂 包括 3 个 旋转 关节 ,每 个 关节 配备 一 个 电机 提供 反馈 力 。 
手柄 是 通过 一 个 万 向 节 安 装 在 机 器 手臂 末端 的 。Touch X 设备 具有 级 联结 构 , 具 有 定位 精 
度 高 .工作 空间 大 ,摩擦 小 .惯性 低 的 性 能 。 

当 操作 者 在 人 机 交互 应 用 上 使 用 触觉 设备 ,如 医学 模拟 训练 ,需要 设计 一 个 视觉 反馈 接 
口 在 模拟 环境 中 显示 用 户 的 操作 来 作为 反馈 信息 ,提高 操作 可 控 性 。 为 了 达到 这 个 目的 , 首 
先 需要 建立 一 个 高 精度 的 机 器 人 运动 学 模型 来 描述 机 器 人 的 运动 。MATLAB 软件 包 提供 
了 一 个 有 效 的 仿真 环境 ,包含 了 多 种 专业 工具 箱 和 许多 数学 函数 ,因此 ,该 软件 包 应 用 于 机 
器 人 实时 的 控制 和 运动 性 能 的 表示 。Robotics Toolbox 是 一 个 在 MATLAB 下 建立 的 机 器 
人 仿真 软件 ,也 已 广泛 应 用 。 它 允许 用 户 以 一 个 简单 的 方式 规划 轨迹 和 实现 坐标 变换 ,同时 
有 内 置 模块 来 获取 机 器 人 动力 学 和 运动 学 信息 ,而 这 些 功 能 都 是 创建 仿真 模型 的 基础 。 

DH 方法 已 广泛 应 用 于 描述 机 器 人 的 运动 学 模型 。 它 可 以 用 来 建立 运动 学 模型 以 分 析 
机 器 人 结构 的 几何 形状 。 如 果 一 个 机 器 人 的 DH 参数 是 完全 已 知 的 ,机 器 人 的 运动 就 可 以 
很 容易 地 利用 机 器 人 工具 箱 进 行 模 拟 , 并 计算 机 器 人 的 运动 学 (例如 雅 可 比 和 矩阵 ,关节 姿态 
等 ) 。 

然而 Touch X 装置 的 标 称 DH 参数 并 不 提供 给 用 户 , 而 且 由 于 机 械 结 构 是 看 不 见 的 ， 
所 以 不 易 测量 。 相 反 ,官方 提供 的 SDK 提供 了 末端 执行 器 的 位 置 和 关节 角度 。 为 了 获得 精 
确 的 DH 参数 并 进一步 建立 运动 学 模型 ,需要 通过 参数 估计 方法 确定 有 效 的 DH 参数 。 

机 器 人 的 自 适应 控制 已 经 研究 了 数 十 年 , 它 一 直 被 视 为 一 个 强大 的 用 来 估计 机 器 人 系 
统 未 知 参 数 的 工具 。 通 过 使 用 恰当 的 自 适应 规律 和 一 些 错误 估计 的 信息 ,估计 的 参数 可 以 
渐 近 收敛 到 真实 值 。 

本 节 提 到 的 自 适应 参数 估计 算法 被 应 用 于 解决 DH 参数 识别 问题 。 同 时 ,利用 DH 表 
示 法 模拟 三 维 运动 学 模型 来 表示 Touch X 的 运动 。 此 外 ,实施 实验 来 测试 模型 的 精确 度 和 
验证 参数 估计 的 有 效 性 。 本 节 建 立 一 个 Touch X 设备 的 精确 模型 ,利用 有 限时 间 收 敛 估 计 
来 确定 未 知 的 DH 参数 和 精确 确定 器 件 的 工作 空间 。 


2.4.2 建 模 步 又 


Touch X 触觉 设备 的 运动 学 建 模 过 程 包括 以 下 步骤 。 首 先 ,介绍 该 触觉 设备 的 基本 机 
械 结构 ; 其 次 ,使 用 修改 的 DH 法 来 获取 Touch X 的 正 向 运动 学 方程 ; 然后 ,利用 自 适应 有 
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限时 间 估 计 的 方法 来 获取 和 触觉 设备 的 DH 参数 。 

1. Touch X 设备 的 描述 

Geomagic 的 Touch X 触觉 设备 由 一 系列 环节 组 成 ,包括 6 个 旋转 关节 。 前 3 个 关节 配 
备 直流 电机 和 光电 编码 器 ,这 些 关节 描述 了 末端 执行 器 的 位 置 和 确定 了 反馈 力 大 小 。 后 3 
个 关节 通过 万 向 节 连 接 到 端 部 执行 器 ,这 些 关节 与 手柄 的 方向 有 关 。 同 时 ,通过 
OpenHaptics 工具 包 , 用 户 可 以 通过 手柄 获取 关节 角度 和 末端 控制 器 的 位 置 。 

2. 机 器 人 正 运动 学 模型 
正 向 运动 学 用 来 描述 关节 角度 和 机 器 人 位 置 和 方向 之 间 的 关系 。 对 于 Touch X 触觉 
设备 ,本 地 化 端 部 执行 器 的 位 置 是 很 重要 的 ,因为 它 代 表 了 手柄 柄 端 并 且 将 用 于 在 虚拟 环境 
中 位 置信 息 的 广义 化 。 同 时 ,手柄 的 方向 与 3 个 转动 关节 形成 的 万 向 节 有 关 。 这 是 一 个 有 
效 的 结构 ,因为 它 允 许 用 户 独 立地 控制 和 定位 。 为 了 建立 一 个 与 Touch X 相似 的 运动 学 模 
型 ,需要 计算 出 它 的 运动 学 模型 的 DH 参数 见 表 2-6. Touch X 的 运动 学 模型 如 图 2-33 所 示 。 


Touch X 模型 的 DH 参数 
xo 杆 aii (m) d; (m) aii (rad) 0: 
1 0 0 0 qı 
2 0 0 —n/2 q: 
3 h 0 0 qs 
4 0 l —x/2 qi 
5 0 0 —n/2 qs 
6 0.030 0 —n/2 qs 


c ,表示 连 杆 的 旋转 ,w ,表示 连 杆 的 长 度 ， i : 
4 表示 连 杆 的 偏 移 , 还 有 0 ,表示 关节 角度 ,它们 um vu 
都 是 变量 。 连 杆 长 度 L 和 Ls 是 关节 2 和 3、 关 节 3 G- ois e 
和 5 之 间 的 长 度 。 在 运动 学 模型 中 ,关节 4 和 5 分 V0 v / 

别 设置 在 两 个 正 交点 的 同一 点 上 。 因 此 , 连 杆 偏 移 ( 

设置 为 零 。 由 于 在 运动 学 模型 中 关节 4.5 和 6 ^ 

只 代表 方向 ,与 末端 执行 器 的 位 置 无 关 。 连 杆 长 度 

ws 可 以 通过 直 尺 来 预先 测量 。 注 意 ， Li， ML 的 长 
度 由 于 机 械 结 构 是 看 不 见 的 因此 是 未 知 的 ,很 难 精 E233 Touch X 的 运动 学 模型 示意 图 
确 地 测量 它 的 值 ,这 些 参数 将 通过 下 一 节 的 估计 算 


法 得 到 。 
利用 DH 表示 法 ,Touch X 设备 的 每 一 个 环节 都 附 有 一 个 坐标 系 。 当 0. 等 于 零 的 时 候 
与 坐标 系 {1} 有 关 ,坐标 系 {2} 的 原点 与 坐标 系 {1} 相 同 。 


有 了 这 些 DH 参数 .有关 ;一 1 到 i 所 涉及 的 坐标 系 的 齐 次 变换 矩阵 如 下 所 示 。 
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Ci —si 0 ai 
了 siCaii cicarl 一 Sarl 一 Sarldi 
mA; = (2. 92) 
Sisarl Cisam crl Cicaiadi 
0 0 0 1 


参考 式 (2.92) 可 以 计算 从 端 部 执行 器 到 基 坐 标的 变换 和 矩阵 ,可 以 通过 以 下 矩阵 乘法 计 

算 所 得 
?A, =A Ar" An (2. 93) 
与 固定 的 参考 坐标 机 器 人 运动 学 模型 不 同 ,Touch X 有 自己 的 世界 坐标 系 。 因 此 ,为 了 
使 得 运动 学 模型 的 直角 坐标 系 与 Touch X 设备 匹配 ,将 一 个 世界 坐标 系 {OW}) 添 加 到 模拟 
的 运动 学 模型 ,如 图 2-34 所 示 。 坐 标 和 矩阵 {0) 到 世界 坐标 系 {W) 的 变换 矩阵 可 以 计算 如 下 


"Ao = RR, (— 5 )R (F) T. 4T. c4 (2.94) 


HPR), RC), RC) € RU 3Gnift xz、y、z 轴 的 齐 次 旋转 变换 矩阵 , T,(。 )， 
T: ORRI yz 轴 的 转换 矩阵 ,dy 和 dz 表示 偏 移 参 数 。R,(。 0. R:(，。) 可 从 实验 获得 ， 
因为 x 方向 没有 偏 移 ,因此 T,(，) 不 考虑 。 


移动 


——| 


自 适 应 参数 估计 


图 2-34 自 适应 参数 估计 过 程 
因此 末端 控制 器 从 坐标 {e} 到 世界 坐标 {W } 的 转换 可 以 通过 以 下 转换 


VA, —V ASA, (2. 95) 
其 中 ,A4。 可 以 如 下 计算 
S3 一 C1 — as pr 
|| Se 9 o9. SS (2. 96) 
Cac 5 asz P. 
0 0 0 1 
Hp s.c ^a 2] XI AR. sing; ,cosqi,sin(g; 十 qj;) 和 cos(g; 十 q;)。p: :py 和 p: 表 示 末 端 执 


行 器 的 位 置 : 
D. =— singicosq;l, 一 lssingisings 
b, = hsinq; — [;cosqs +d, 
D. = l;cosqicosqs + l;cosqisinqs + d, (2. 97) 
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式 (2.97) 中 的 运动 学 方程 可 以 用 一 个 紧凑 的 形式 重 写 为 
p =) (2. 98) 
其 中 ,p= 二 [p: ,py,p:] ”是 连续 函数 向 量 ,g € R3, 是 关节 向 量 。 
这 样 ,建立 了 Touch X 的 正 向 运动 学 和 它 的 位 置 跟 关节 角度 的 关系 。 
末端 执行 器 的 笛 卡 儿 位 置 可 以 通过 正 向 运动 学 模型 计算 所 得 。 然 而 ,制造 商 不 提供 触 
觉 设 备 的 DH 参数 。 因 此 采用 了 一 种 自 适应 的 有 限时 间 参 数 估计 方法 来 确定 这 个 DH 参 
数 ,以 获取 精确 的 值 来 建立 运动 学 模型 。 在 估算 之 前 ,将 式 (2. 98) 改 写 为 : 
p=$(00 
其 中 ,0 二 [4 ,ls,d,,d:]" "ER', 是 未 知 的 参数 向 量 。 而 多 (gq)€ Rs 是 已 知 的 回归 矩阵 。 根 
dip. ,py，p: 和 0 的 定义 ,多 (q) 可 以 定义 如 下 
一 Singicosq* — singisings 0 0 


690 一 singz — cosqs 1.0 (2. 99) 


cosq;cosq; 一 ] cosqsings 0 1 

估算 的 目的 是 使 得 (7) 接近 它 的 真实 值 9, 确保 位 置 误差 p= 二 p 一 的 收敛 性 。 其 中 ， 
p=$(q)6 是 真实 位 置 p 的 估算 。 为 了 估算 方便 ,一 个 辅助 过 滤 和 矩阵 DE Ri fit FE 
Ri! 介绍 如 下 


| =—hD+$"$, D(0)=0 
. (2. 100) 
F=—hF+$ p, FO) =0 
其 中 ,h 是 一 个 由 设计 者 指定 的 参数 。 
对 上 式 进 行 积分 ,结果 如 下 
Di) = [os GG)" $(qtr))dr (2.10) 
FQ) = feg (gr) pGodr 
对 比 式 子 中 的 D 和 下 ,再 根据 p 的 定义 ,可 以 得 到 下 式 
F = D0 (2.102) 
可 以 定义 由 D、F 所 确定 的 误差 向 量 W 
Ww -— DO—F (2.103) 
通过 参数 估计 误差 的 定义 6 二 0 一 6.W 可 重 定义 为 
w = Dó — F = Dó — D0 = DO (2.104) 
这 表明 ,向 量 W 包含 了 位 置 误差 的 信息 ,之 后 自 适应 参数 估计 法 则 如 下 
ĝ=-p DW (2.105) 


Piw] 
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其 中 , 研 是 一 个 常数 学 习 增 益 矩 阵 。 

定义 ”假设 一 个 向 量 或 矩阵 函数 上 是 持续 激发 的 ,如 果 存 在 工 二 0, € 二 0, 那 么 

T 

iT $6) dr z el. VEZ 0. 

定理 ”使 用 pg(q) 满 足 PE 条 件 的 运动 学 模型 和 参数 估计 法 则 ,估计 误差 会 在 有 限时 间 内 
收 伍 ta, WE tas 16(o) | | Amax Q* — 1» /s. Amax( * 2 JEE [e — A d B FEE f -AmaxC e ) 
2970.5 是 一 个 正定 值 。 

WEBB. 考虑 李 雅 普 诺 夫 函 数 : 


V= iri (2.106) 
考虑 上 式 对 时 间 的 偏 导数 ,可 得 
v= Im =- 5" Dw 
I D6 — F | 

__ 0*D'Dó 

I D || 
<- | D | <- y W (2. 107) 

HP u50 VAn A O EAER c. E fi oc Sc f ERNE. 


2.4.3 仿真 设计 


下 面 介绍 Touch X 运动 学 建 模 的 步 又。 基于 自 适应 参数 识别 的 程序 实现 步骤 框图 如 
图 2-35 所 示 。 


( uzas | 一》 sexe ] 


Tb 
设 定 各 参数 
( 一 《< 一 | 读 取 数据 ) 


ARR 自 适应 参数 
f 微分 方程 ”一 “估计 


绘制 相关 曲线 zi 
图 形 《< 一 | 存储 数据 


2-35 程序 的 实现 步骤 


根据 程序 的 实现 步骤 ,基于 自 适应 参数 识别 的 Touch X 运动 学 建 模 进行 编程 ,代码 如 
下 所 示 : 
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data p(k0, :) = p'; 

data theta(k0, :) = theta'; 
data dtheta(k0, :) = dtheta'; 
data W(k0,:) = W'; 

data ep(k0,:) = ep'; 

end 


figure(1) 
plot(t,data ep(:,1), 'b- ');hold on; 
plot(t,data ep(:,2), 'r- '); 
plot(t,data ep(:,3), 'g- ');hold off; 
legend('ep x', 'ep y','ep z'); 
figure(11) 
plot3(q(:,9),3(:,7),3(:,8), 'b—- ', 'LineWidth',1);hold on; 
* plot3(data p(:,3),data p(:,1),data p(:,2), 'g- ', 'LineWidth',1); 
plot3(q(1,9),q(1,7),a(1, 8) ', 'rd', q(end, 9) , a( end, 7) , q( end, 8) ', 'bs') ; hold off; 
legend('Actual trajectory', 'Start point', 'End point') 
xlabel('z(n)'); 
ylabel('x(m)'); 
zlabel('y(m)'); 
figure(2) 
plot(t,data theta(:,1), 'b- ');hold on; 
&plot(t,data theta(:,2), 'r- '); 
plot(t,data theta(:,3), 'g- ');hold off; 
legend('a 1','a 2'); 
figure(11) 
plot(t,q(:,7), 'b- ');hold on; 
plot(t,q(:,8),'r- '); 
plot(t,q(:,9), 'g- '); hold off; 
legend('x', 'z', 'z'); 
figure(12) 
plot(t,data p(:,1), 'b- ');hold on; 
plot(t,data p(:,2),'r- '); 
plot(t,data p(:,3) — 0.11, 'g- '); hold off; 
legend('x', 'z','z'); 


2.4.4 实验 和 仿真 


为 了 说 明 DH 参数 估计 的 有 效 性 ,以 下 进行 数值 仿真 和 实验 。 实 验 的 设计 是 为 了 验证 
估计 的 DH 参数 和 建立 的 运动 学 模型 的 有 效 性 。 

1. DH 参数 估计 

为 了 执行 估计 算法 ,满足 PE 条件 的 真实 的 位 置 轨迹 和 关节 角度 通过 操作 伴随 一 个 贺 
形 轨迹 的 末端 执行 器 广义 化 。 在 估算 之 前 ,参数 缩减 方法 被 应 用 于 简化 估算 过 程 ,并 根据 连 
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杆 长 度 和 坐标 偏 移 之 前 的 关系 提高 计算 效率 。 在 Touch X 设备 的 运动 学 模型 中 , 偏 移 参数 
与 连 杆 长 度 dy ,dz 有 关 , 相 关 方程 dy= /2 一 0.1,dz 王 一 上 L。 因 此 ,估算 的 参数 可 以 平等 地 
简化 为 0 = [L1, 12]。 在 接 下 来 的 模拟 中 ,ll 和 /2 的 初始 值 被 设 定 为 (0) = [41，22] 一 
Lo. 0] ,而 自 适应 学 习 的 增益 设置 为 = 21, 还 有 有 被 设 定 为 h — 10. 仿真 结 果 显示 在 
图 2-36 和 图 2-37 中 ,图 2-36 顶部 的 图 像 显 示 了 参数 /1 估计 的 效果 ,而 底部 显示 参数 12 
估计 的 效果 。 而 图 2-37 展示 了 真实 数据 与 所 建 模型 计算 数据 的 位 置 误差 。 可 以 从 图 中 看 
到 ,参数 收敛 到 一 定 值 时 ,同时 位 置 误差 收敛 到 原点 。 这 表明 自 适应 有 限时 间 参 数 估计 算法 
可 以 实现 高 精度 的 估计 ,并 保证 在 短 时 间 内 收 僵 。 因 此 ,Touch X 的 估计 所 得 的 参数 为 
11 = 0.13335m, /2 =0. 13335m, 


估计 值 (m) 
g 


6 8 10 12 14 qs) 16 


N 
上 上 


图 2-37 自 适 应 DH 参数 估计 
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2. 实验 证 明 

为 了 验证 估计 的 DH 参数 的 准确 性 和 运动 学 模型 的 有 效 性 ,进行 了 实验 进行 证 明 。 在 
验证 之 前 ,移动 手柄 ,并 通过 读 取 官方 的 软件 开发 包工 具 来 广义 化 Touch X 的 关节 角度 和 
位 置 轨迹 。 然 后 利用 运动 学 方程 来 计算 笛 卡 儿 坐标 下 的 位 置 与 DH 参数 的 估计 。 实 验 结 果 
如 图 2-38 所 示 , 通 过 真实 数据 和 从 运动 学 模型 计算 所 得 的 位 置 轨迹 信息 得 到 展示 。 从 
图 2-38 可 以 看 出 , 当 实 际 机 器 和 所 建立 模型 之 间 的 误差 收敛 到 零 时 ,由 运动 学 模型 产生 的 
轨迹 (红色 曲线 ) 与 Touch X 末端 执行 器 的 实际 轨迹 很 接近 。 因 此 ,估计 DH 参数 的 准确 性 
和 运动 学 模型 的 有 效 性 得 到 了 验证 。 为 了 实现 模拟 仿真 目的 的 运动 学 模型 的 精度 是 令 人 满 
意 的 。 


04| ”一 实际 轨迹 
— 模型 轨迹 
一 偏差 


一 一 实际 轨迹 
一 一 模型 轨迹 
r3 


Xm) 


z(m) 


t(s) 
图 2-38 实际 位 置 轨 迹 与 运动 学 模型 广义 轨迹 的 比较 
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2.4.5 可 视 化 运动 学 模型 与 工作 空间 识别 

利用 DH 参数 和 运动 学 方程 ,Touch X 的 可 视 化 运动 学 模型 是 通过 MATLAB 中 的 机 
器 人 工具 箱 创建 的 。 同 时 ,通过 仿真 研究 已 确定 Touch X 的 工作 空间 。 

1. 可 视 化 的 运动 学 模型 

根据 已 建立 的 运动 学 模型 ,Touch X 可 视 化 运动 学 模型 是 用 机 器 人 工具 箱 建立 的 , 见 
图 2-39。 当 所 有 关节 角度 已 知 的 时 候 ,可 以 利用 运动 学 模型 计算 出 Touch X 的 位 置 和 方 
向 。 如 果 给 定 了 相同 的 关节 角度 , 当 相同 的 Touch X 姿势 出 现时 ,将 在 可 视 化 界面 上 给 出 。 
这 意味 着 通过 可 视 化 的 模型 ,可 以 模拟 运动 ,并 进一步 研究 Touch X 设备 的 属性 。 


Mm) 


图 2-39 Touch X 实际 图 与 可 视 化 模型 


2. 工作 空间 的 确定 

利用 运动 学 模型 和 机 器 人 工具 箱 确定 Touch X 的 工作 空间 时 ,机械 手 的 工作 空间 边界 
估计 是 机 器 人 算法 总 体 设计 和 分 析 优化 必 不 可 少 的 。 利 用 运动 学 模型 和 DH 参数 和 关节 旋 
转 极 限 , 可 以 延伸 蒙特 卡 洛 法 来 识别 Touch X 的 活动 空间 。 均 匀 的 径 向 分 布 是 用 来 产生 
6000 个 Touch X 随机 选择 的 关节 角度 值 的 点 ,并 应 用 运动 学 估计 该 末端 执行 器 的 位 置 。 因 
此 ,Touch X 可 达 的 工作 空间 点 云 创建 如 图 2-40 所 示 。 接 下 来 ,用 Delaunay 三 角 来 生成 一 
组 在 三 维 空间 点 的 外 接 圆 。 这 有 利于 创建 一 个 用 于 约束 工作 区 的 凸 壳 的 联合 空间 。 识 别 到 
Touch X 的 3D 工作 空间 如 图 2-41 所 示 。 因 此 ,Touch X 的 工作 空间 已 被 确定 。 


图 2-40 ”2D 空间 下 表示 Touch X 的 工作 区 间 图 2-41 3D 空间 下 表示 Touch X 的 工作 区 间 
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2.5 复杂 扰动 环境 中 的 新 型 机 械 臂 混合 自 适应 控制 器 


本 节 介 绍 复 杂 扰 动 环境 中 的 新 型 机 械 臂 混合 自 适应 控制 器 的 控制 问题 ,仿真 设计 ,编程 
与 实现 。 


2.5.1 引言 


我 们 期 望 现代 机 器 人 能 够 与 环境 和 人 类 广泛 地 交互 。 当 与 动态 的 未知 的 环境 交互 时 ， 
我 们 需要 一 种 控制 方法 ,即使 受到 了 干扰 ,仍然 能 够 保持 稳定 性 并 高 效 地 执行 任务 。 阻 抗 控 
制 是 人 们 首次 提出 与 位 置 环境 交互 的 控制 方案 之 一 。 环 境 被 建 模 为 导 纳 系统 ,机 械 辟 被 建 
模 为 阻抗 系统 ,使 得 交互 式 控制 能 够 通过 能 量 交 换 实现 。 阻 抗 控制 能 够 在 自 适应 控制 之 上 
进行 设计 ,从 而 补偿 了 参数 的 不 确定 性 。 自 适应 阻抗 控制 改善 了 传统 的 阻抗 控制 器 的 操作 
性 能 。 特 别 地 ,控制 器 能 够 在 初始 交互 不 稳定 (典型 的 情况 如 钻 孔 、 雕 刻 ) 的 情况 下 ,逐渐 保 
持 稳定 和 成 功 执行 任务 。 

与 上 述 的 研究 并 行 发 展 ,在 生物 学 和 人 类 神经 学 等 领域 ,研究 已 经 表明 人 类 的 神经 系统 
能 够 适应 机 械 阻抗 (例如 , 抗 扰动 ) ,以 在 稳定 和 不 稳定 的 环境 中 成 功 执行 任务 。 这 是 通过 如 
图 2-42(a) 中 所 示 的 “主动 肌 / 对 抗 肌 ” 肌 肉 群 共同 收缩 实现 的 。 神 经 系统 通过 独立 控制 阻抗 
和 施加 的 力 来 适应 运动 命令 以 稳定 交互 作用 ; 适应 过 程 自动 选择 激活 合适 的 肌肉 以 补偿 交 
互 作用 和 不 稳定 性 。 同 时 , 当 误 差 足够 小 时 ,通过 肌肉 群 自然 松弛 使 代谢 成 本 最 小 化 。 这 种 
学 习 模 型 演化 成 了 一 种 新 型 的 非 线 性 自 适应 控制 器 ,这 种 控制 器 已 成 功 地 应 用 在 机 器 人 上 。 
该 仿生 控制 器 中 阻抗 的 适应 遵循 *V 形 ? 散 发 ,如 图 2-42(b) 所 示 。 常 规 设计 的 自 适应 控制 


伸 肌 SN 届 肌 2N 
改变 电机 命令 


增加 共和 激活 


之 前 实验 的 误差 


(a) (b) 


图 2-42 共同 收缩 如 何 影响 到 肌肉 阻抗 
Ca). 通过 两 侧 肌肉 以 不 同 的 力 同时 收缩 , 届 肌 和 伸 肌 肌肉 一 起 工作 以 维持 效应 器 扭矩 ,增加 阻抗 ;(b)“V- 形 " 自 
适应 律 。 阻 抗 的 增加 与 误差 方向 无 关 , 当 误差 小 于 一 定 阔 值 时 阻抗 减 小 ,这 种 机 制 保证 了 代谢 成 本 ( 即 控制 力 ) 
的 最 小 化 


182 4| 机 器 人 仿真 与 编程 技术 


器 设计 通常 关注 于 在 稳定 的 运动 下 对 不 确定 参数 进行 估计 ; 相 比 之 下 ,仿生 控制 器 设计 能 
够 通过 力 和 阻抗 的 适应 在 不 稳定 动力 学 下 获得 稳定 ,同时 使 控制 能 量 最 小 化 。 类 似 于 肌肉 
松弛 ,在 稳定 的 相互 作用 下 ,控制 器 也 表现 出 柔顺 性 ,这 在 最 近 的 机 器 人 操作 研究 中 得 到 很 

以 下 从 两 方面 拓展 这 种 新 型 的 自 适应 控制 器 : 第 一 个 方面 是 “任务 /关节 ”空间 混合 控 
制 。 通 常情 况 下 ,控制 器 要 么 在 关节 空间 中 实现 (对 应 于 驱动 器 ) ,要 么 在 笛 卡 儿 空 间 中 实现 
(在 这 种 情况 下 必须 求解 逆 运 动 学 )。 这 两 种 控制 方法 都 有 优势 和 劣势 : 

CD 相 比 于 关节 空间 控制 器 , 笛 卡 儿 控 制 器 能 够 在 世界 空间 中 更 加 直观 地 表达 轨迹 。 
放置 在 工作 空间 中 的 物体 通常 具有 笛 卡 儿 表达 式 ,例如 一 个 箱子 放置 在 机 器 人 前 0. 1m 处 。 

(2) 另 一 方面 ,机 器 人 通常 需要 在 关节 空间 中 输入 ,也 就 是 说 ,输入 是 力矩 而 不 是 力 或 
动量 。 因 此 ,在 关节 空间 中 进行 控制 避免 了 递 运动 学 的 问题 ,使 得 其 计算 量 要 比 在 笛 卡 儿 空 
间 中 进行 控制 的 计算 量 小 。 对 于 欠 驱 动 或 元 余 机 器 人 (例如 Baxter 机 械 手 ?来 说 ,更 是 
如 此 。 

(3) 远程 操作 任务 在 关节 空间 中 会 更 加 直观 ,如 当 一 个 拟人 机 器 人 模拟 人 来 操作 者 时 。 

(4) 更 具体 地 分 析 在 关节 空间 中 进行 控制 能 够 使 机 械 臂 鲁 棒 性 更 好 ,能 够 通过 监控 关 
节 空 间 误 差 来 抵抗 作用 于 机 械 臂 任意 部 分 的 干扰 。 笛 卡 儿 控制 器 对 于 发 生 在 机 械 臂 末 端 执 
行 器 处 ,任务 特定 的 干扰 很 敏感 。 

因此 ,本 节 内 容 开 发 并 研究 了 关节 - 笛 卡 儿 空 间 混 合 控 制 方案 ,以 利用 这 两 种 控制 方法 
的 优点 。 我 们 研究 的 笛 卡 儿 空间 任务 是 沿 着 给 定 轨迹 搬运 一 个 物品 ,同时 在 执行 器 末端 或 
是 沿 着 机 械 臂 (或 两 者 ) 施 加 扰动 ,类 似 于 在 喧哗 的 房间 握 着 一 杯 香槟 时 抑制 噪声 。 

自 适应 控制 较 少 受到 关注 的 另外 一 方面 原因 是 学 习 参 数 的 设置 。 这 些 参数 通常 是 由 使 
用 者 整定 的 ,以 便于 完成 一 项 任务 和 提高 性 能 ,例如 减 小 跟踪 误差 。 自 动 选择 学 习 参 数 不 是 
一 项 简单 的 任务 。 真 实 世 界 中 的 机 械 臂 系统 由 于 与 环境 进行 交互 ,从 而 表现 出 复杂 的 ,未 知 
的 动力 学 特性 ,这 使 得 建 模 很 困难 ,或 者 在 某 些 情况 下 完全 无 法 建 模 。 基 于 神经 网 络 的 方法 
可 以 用 来 估计 不 确定 性 ,从 而 避免 其 中 的 一 些 难题 。 然 而 ,模糊 逻辑 可 以 用 于 转移 来 自 人 类 
操作 者 的 专业 知识 ,以 便 在 面 对 不 精确 数据 时 做 出 理性 的 决策 。 模 糊 多 辑 已 经 成 功 地 引入 
到 控制 系统 中 以 提高 性 能 ,并 且 最 近 已 被 用 于 非 线性 系统 和 机 器 人 操作 。 因 此 本 节 内 容 开 
发 了 一 种 基于 模糊 逻辑 的 方法 来 设置 学 习 参 数 。 

本 节 内 容 中 的 一 些 概念 将 会 在 Baxter 机 器 人 的 单 臂 上 进行 仿真 和 测试 ( 见 图 2-43) 。 
Baxter 是 一 个 双 臂 .造价 低 的 机 器 人 ,由 Rethink Robotics 公司 设计 ,广泛 应 用 于 工业 应 用 。 
用 于 学 术 研 究 的 Baxter 机 器 人 已 经 面世 ,可 以 购买 。 


2.5.2 控制 问题 


Baxter 要 求 在 末端 执行 器 以 高 频 、 低 振幅 震动 的 影响 下 (仿真 工具 可 能 产生 的 干扰 类 
型 ), 沿 着 给 定 轨 迹 移 动 。 此 外 .还 有 一 个 高 振幅 \ 低 频 的 扰动 作用 在 辟 上 远离 末端 执行 器 的 
一 点 ,用 以 模拟 机 械 臂 与 操作 员 或 环境 的 碰撞 。 表 2-7 给 出 了 变量 的 命名 以 供 参 考 。 
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图 2-43 Baxter 手臂 和 作用 于 该 手臂 的 干扰 力 
ik: 作用 在 末端 执行 器 的 Fex 以 及 作用 在 远离 末端 执行 器 的 Fone 使 用 MATLAB T RAS" MATLAB 
and Peter Corke' s Robotics Toolbox” 生 成 的 模型 如 右 图 所 示 。 


R27 变量 命名 
符 ”号 LEE: 

n 关节 数量 , 即 自由 度 
qe» 关节 角度 
gew 关节 角速度 
"Ib 关节 角 加 速度 
XER 笛 卡 儿 / 任 务 空间 位 置 
xe» 第 卡 儿 /任务 空间 速度 
Xe» f E JL/ FE 4e 2 i] Jm i E 
d*sX* 期 望 关节 位 置 , 笛 卡 儿 位 置 
Fu Fenn € R? 分 别 为 内 力 , 外 力 
ME PR'”" 惯量 矩阵 
ce» 科 氏 和 离心 力 
GER 重力 
PLI 输入 力矩 
ta EW 扰动 力矩 
neo 参考 力矩 
PI 关节 空间 控制 力矩 
ne» 任务 空间 控制 力矩 
LER” 稳定 裕 度 
JER” 机 械 臂 的 雅 可 比 矩 阵 
J ER” FETI HR De Doa 


ZE Rs 简化 矩阵 
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Ex 
符 号 描 $ 
DS 分 别 为 关节 空间 和 任务 空间 位 置 误差 
ever 分 别 为 关节 空间 和 任务 空间 速度 误差 
Ej. 分 别 为 关节 空间 和 任务 空间 跟踪 误差 
E 绝对 值 
We 欧 几 里 得 矢量 范 数 
Otxa iXj t oE 
1. 机 器 人 动力 学 
机 器 人 手臂 的 动力 学 表达 式 如 下 : 
Mq) 0 十 C(qg,0) 4 GID = Pu + Fais (2. 108) 


其 中 ,gq 表示 关节 角 向 量 ,M(q)€ WR*" 是 对 称 、 有 界 、 正 定 惯 性 矩阵 ,n 是 机 器 人 手臂 的 自由 
度 ; C(q.q)q EW 表示 科 氏 力 和 离心 力 ; GGD € 9 是 重力 ; e, € 9 是 控制 输入 力矩 矢量 ; 
tí. € 9 是 由 摩擦 力 、 环 境 干扰 或 负载 引起 的 扰动 转 矩 。 控 制 力 矩 w, 由 设计 的 控制 器 产生 ， 
以 便 在 运动 跟踪 和 扰动 抑制 方面 实现 期 望 的 性 能 。 

2. 扰动 

假设 扰动 转 矩 mis 可 以 分 解 为 两 个 分 量 ,以 模拟 在 未 端 执 行 器 的 任务 干扰 (这 里 描述 为 
Fus) 和 施加 在 臂 上 的 环境 干扰 Fenn o 

Fax =[p 0 0 0 0 0]'. p= A,sin(2zxoy) (2.109) 

作用 在 末端 ,其 中 ON A,SL20N 表示 幅 值 .100 二 ws 三 1000 是 以 Hz 为 单位 的 振荡 频率 。 
在 关节 空间 中 ,施加 的 扭矩 如 下 : 


fas = JI (Q) F usk (2.110) 
其 中 , 雅 可 比 矩 阵 J(q) 由 zz 三 J(g)g 定 义 。 环 境 干扰 如 下 : 
Fon=[r 0 0 0 0 0]"', r= A,sin(2xw) 《2.111) 


Kp 20N— A, 100N 是 扰动 幅 值 ,类 似 于 人 类 推 / 拉 强 度 的 平均 极限 ,0. 1 过 w 1 是 以 
Hz 为 单位 的 频率 值 , 它 提供 了 缓慢 变化 的 扰动 。 为 了 模拟 作用 在 辟 上 一 点 的 环境 力 Fas 
(例如 在 肘 部 位 置 ) , 雅 可 比 窍 阵 了 被 矩阵 Z 简化 ,矩阵 乙 定 义 为 ， 


m Itxa 
JE Pad (2.112) 
其 中 ,= Ji A JE p 8] E fh e (0 35 A Cf n. An R 7] d JL E JE AEA z— 40. FRHEJIAB UT 


以 通过 下 式 获得 : 
gov = (JZ) Fan (2.113) 
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2.5.3 自 适 应 控制 


1. 前 馈 控制 器 
考虑 到 机 械 臂 的 动力 学 特性 ,采用 以 下 控制 器 作为 初始 输入 力矩 ， 
£D =M) d^ CQ.) d' HAD — LDE) (2.114) 


其 中 ,LCDel4) 对 应 于 产生 最 小 反馈 (类 似 于 肌肉 和 肌 腿 的 被 动 阻抗 效应 ) 的 期 望 的 稳定 裕 
度 , 并 且 前 三 项 是 用 于 机 械 辟 动 力学 的 前 馈 补偿 。 参 考 滑 模 控制 ,使 用 如 下 跟踪 误差 : 


Eet) = è) + ke) (2.115) 
其 中 
eD =D D, ÈD = 4() —4^ Q) (2.116) 
分 别 为 关节 角度 和 角速度 误差 。 除 了 上 述 控制 输入 g, O ,本 节 开 发 了 关节 空间 和 任务 空间 
的 自 适应 。 


1) 关节 空间 自 适应 控制 
用 于 整定 控制 力矩 gw. 的 前 馈 和 反馈 分 量 的 自 适应 律 适用 于 关节 空间 和 任务 空间 。 这 
里 自 适应 在 运动 期 间 是 连续 的 ,而 不 是 一 次 接着 一 次 地 重复 运动 ,因此 跟踪 误差 和 控制 力 被 
连续 地 最 小 化 。 定 义 以 下 公式 : 
t£ 一 一 8 (1) Ke) — DA) CC) (2.117) 
其 中 ,一 e (1) 是 学 习 获得 的 前 馈 力矩 ,一 K(COD)e(O 和 一 DCD)e(2) 是 分 别 对 应 于 刚度 和 阻尼 的 
反馈 力矩 。 用 于 周期 为 工 的 轨迹 跟踪 的 自 适应 律 如 下 : 
ðe (D — ei) —egO— T) =Q) ya) (1)) 
SK 0) = K0) — KG — T) = QkCeCOe' (4) — YCOKCO) (2.118) 
D(t) = DG) — DG — T) = Qo lelt) é" (0 — y(GODGD) 
将 遗忘 因子 y(7) 从 增益 矩阵 Q..,) 中 分 解 出 来 ,以 避免 当 yY(z) 和 Q.., 都 比较 大 的 时 候 产 生 的 
高 频 振 荡 。 如 上 所 述 , 考 虑 连续 时 间 的 适应 ,而 不 是 通过 连续 迭代 试验 的 适应 ,导出 了 关节 
空间 中 的 自 适应 律 : 
ðe (4) =g 4t) — e 0 — åt) = Qe — y(t) e CO 
òK (t) = KG) — KG — ôt) = Qr; (Def CO — Y; OK; Q) 


ôD (t) = DG) — DG — ôt) = Qe; CO ej GO) — yr OD; GO (2. 119) 
其 中 ,6: AE FEISE BI K; (00 = 0553 «D; (CO = 0px. Qis Qu Qv € 9" EX fA IE zE HE a E 
VE, It) .y CO € R S E fü OB P. Hou E 


Eu a 
nO-irr (2.120) 
上 式 需 要 整定 两 个 变量 : a 和 4。 为 了 简化 参数 的 选择 ,重新 定义 y 值 如 下 
2 
7:0) = axexp(— HR )，0%< mi 和 1 (2.12D 
. lag 


上 式 只 需要 调整 一 个 变量 a; 来 描述 7 取 值 随 着 s 变化 的 曲线 形状 , 见 图 2-44, 同 时 保持 着 
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与 前 面 式 子 相同 的 功能 。 这 也 展示 了 简单 应 用 模糊 推理 机 的 优点 ,后 文 会 进一步 描述 。 
—a-| a 对 7 的 影响 
=0.5 
-a=02 


0 上 -= = 
-0.1 -0.04 0 0.04 0.1 
a 


图 2-44 a 的 大 小 如 何 影响 遗忘 因子 y 
注 : a 取 较 高 值 时 ,y 为 高 窄 的 形状 ,从 而 在 跟踪 性 能 良好 时 ,控制 力 能 够 最 大 限度 地 减 小 ; 当 跟 踪 性 能 较 差 时 ， 
遗忘 因子 较 小 ,反馈 力矩 随 之 增加 。 
2) 任务 空间 自 适应 控制 
任务 空间 控制 器 的 设计 与 关节 空间 中 的 设计 方式 类 似 。 首 先 , 在 笛 卡 儿 空 间 中 定义 误 
PU 
e(t) = XM) — X'W) 
élt) = X00 — X (OO (2.122) 
e (4) = é, GO + ke, Q) 
这 使 得 前 面 公式 中 描述 的 前 馈 和 反馈 项 发 生变 化 ,为 : 
ÒF. = Qres — Y-F: 
oK, = Qei — Y-K- (2.123) 
òD: = Que. 6: — y.D. 
从 而 
一 JT(q)( 一 下 .一 Ke 一 Dec) (2.124) 
对 于 任务 空间 的 遗忘 因子 定义 加 下 


Ew ) 


y. = aexp(- $ uz) 0<a <1 (2.125) 
. das 


3) 混合 控制 器 
基本 控制 器 ,关节 空间 控制 器 和 任务 空间 控制 器 相互 组 合 后 产生 了 混合 控制 器 ,因此 输 
入 力矩 为 : 
£1) = Aa Aa l AED) (2.126) 
KB 4g € 977 don ME E HAA AE Pe . (EKI J E Bo i R dil EREK 
上 ,这 些 关节 取决 于 所 需 执行 的 任务 。 假 设 能 够 获得 机 器 人 精确 的 动态 模型 ,由 扰动 产生 的 
力矩 ga 给 定 为 
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Tais = M(g) g++ Cg.9) +G) 一 gw (2.127) 
HI , 建 模 的 系统 力矩 减 去 输入 力矩 。 通 过 相对 于 该 矢量 最 大 元 素 归 一 化 该 力矩 矢量 ,可 以 形 
成 加 权 矢 量 : 
Bast 
DD 
然后 将 上 式 应 用 到 前 面 的 公式 中 ,使 得 关节 空间 控制 力矩 能 够 主要 施加 在 那些 受到 较 大 扰 
动力 影响 的 关节 上 , 较 少 施加 在 那些 受 影响 较 少 的 关节 。 这 限制 了 不 必要 施加 的 控制 力 , 减 
少 了 总 的 控制 力 。 
2. 控制 增益 的 模糊 推理 
传统 意义 上 ,用 户 基 于 系统 运行 时 的 响应 来 设置 学 习 参 数 Q.… 和 ,以 确保 良好 的 控制 
性 能 。 在 这 里 ,系统 的 专家 知识 被 提炼 成 模糊 推理 机 以 在 线 调 整 增益 ,从 而 不 需要 用 到 先 验 
知识 。 系 统 性 能 得 到 改进 ,因为 系统 将 根据 对 不 可 测 干扰 的 系统 响应 来 选择 适当 的 增益 值 。 
推理 是 根据 跟踪 误差 和 控制 力 得 到 的 ,期 望 该 推理 能 够 使 跟踪 误差 和 控制 力 最 小 化 ,同时 给 
出 控制 器 总 体 性 能 的 良好 指示 。 
输出 Y 的 模糊 推理 需要 执行 几 个 步 又。 首先 ,模糊 化 ,利用 隶属 度 函 数 ,将 真实 的 标量 
(例如 ,温度 值 ) 映 射 到 模糊 空间 中 。 令 X 为 点 的 空间 ,空间 中 的 元 素 r€ X. X 中 的 模糊 集合 
人 A 通过 将 区 间 [0,1] 中 的 隶属 度 pa (xi) 与 A 中 的 每 个 点 工 相关 联 的 隶属 度 函 数 ya (x) 来 描述 。 
本 节 使 用 简单 的 三 角形 隶属 度 函数 , 它 对 输入 变化 的 灵敏 度 低 ,并 且 计 算 成 本 低 。 此 
外 ,所 有 的 隶属 度 函 数 都 被 设置 ,使 得 所 有 的 模糊 集合 的 完整 度 是 0.5。 这 通过 消除 论 域 中 
真实 程度 较 低 的 区 域 来 降低 不 确定 性 ,保证 了 合理 的 超 调 。 
需要 先 定义 几 个 概念 。 集 合 A 和 集合 B 的 并 集 是 模糊 集合 C, 对 应 于 “或 ”连接 方式 如 下 。 


0; =0G j) (2.128) 


C=AUB; pelz) = max lz) pa (2)]. z€ X (2.129) 
交集 ,对 应 于 “和 ”连接 方式 ,能 够 类 似 地 描述 如 下 : 
C=ANB; pela) = min[(ua(z)vpua(z))]，zEX (2.130) 


笛 卡 儿 乘积 用 于 描述 两 个 或 更 多 的 模糊 集 之 间 的 关系 , 令 A 为 X 空间 中 的 集合 ,B H Y Z 
间 中 的 集合 。 集 合 A 和 集合 B 的 笛 卡 儿 乘积 结果 满足 以 下 关系 : 


R=AXBCXXxXY (2.131) 
其 中 ,模糊 关系 R 具 有 一 个 隶属 度 函数 
AR(CZTyy) = paxp (TY) = min( pa Cr) ,pa(y)] (2.132) 


上 式 在 Mamdani 最 小 推理 中 将 用 到 ,用 于 将 输出 集合 与 输入 集合 相关 联 , 即 ,如 果 工 是 A， 
那么 y 是 B。 然 后 使 用 规则 集 来 表示 输出 ,这 是 所 有 规则 最 大 的 聚合 。 
接着 使 用 一 般 的 重心 法 执行 去 模糊 化 操作 。 去 模糊 后 的 输出 y* 计 算 如 下 


TN IDEE 
IS 
上 式 计算 聚集 的 输出 素 属 度 函 数 的 质心 ,关联 y 值 使 y 返回 到 清晰 的 输出 值 。 


y (2.133) 
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模糊 系统 原始 输入 为 跟踪 误差 和 控制 器 控制 输出 ,在 关节 空间 中 为 sj,r; 类似 地 ,在 
任务 空间 中 为 e: , F,。 在 进行 模糊 化 操作 前 ,必须 对 输入 进行 归 一 化 处 理 , 使 得 同样 的 模糊 
推理 机 具有 一 般 性 而 不 取决 于 输入 的 幅 值 。 跟 踪 误 差 6; ER E ERATI c, € OP 和 
i AIF, € 并 的 平均 基准 值 通过 在 总 的 仿真 时 间 步 iy/6t 中 ,针对 每 个 自由 度 进行 计算 获得 ， 
a ed, -ls NP PEE BÉ MIFCOI 


95 gja ° h 1,/8t Te t/t ^ 6" 1716t 
(2.134) 
然后 将 这 些 用 于 计算 模糊 系统 的 输入 , 即 给 出 与 前 一 次 迭代 相 比 的 性 能 指示 的 值 。 
E eleXC | » ole W]  - a | Ta C) | o| F,C)| 
zD 1. &0 ,DO FD = 
&j, £s Tu Ê., 
(2.135) 


对 于 模糊 控制 系统 的 所 有 输入 ,小 于 的 值 表示 性 能 的 改善 ,大 于 的 值 表示 性 能 变 差 。 在 
这 里 ,我 们 设置 c 一 0.5, 从 而 输入 的 范围 大 约 在 0 一 1 之 间 。 式 (2. 135) 产 生 的 变量 没有 上 
界限 ,因此 任意 一 个 大 于 单位 的 输入 都 会 返回 “高 ”分 类 的 最 大 真 值 。 这 允许 将 一 组 通用 的 
输入 隶属 度 函数 应 用 于 所 有 系统 中 。 
这 些 被 归 一 化 的 变 基 接 下 来 被 应 用 于 自 适应 规律 。Q, 三 Q; E;F) ,Qu — Qi, (EF) 
Qo —Q», (E; 2) ,a 三 Qj E; ,Tj) 用 于 关节 空间 控制 器 ,同时 能 够 应 用 于 任务 空间 控制 器 中 。 
使 用 专家 知识 设置 控制 增益 的 模糊 推理 规则 。 通 常情 况 下 : 如 果 控 制 力 太 高 ,那么 增 
益 设 置 为 低 ; 如 果 跟 踪 误 差 性 能 太 差 ,那么 增益 设置 为 高 (如 表 2-8 所 示 对 Q. .进行 设置 )。 
遗忘 因子 增益 对 应 的 真 值 表 则 有 所 不 同 ( 参 看 表 2-90 : 当 误 差 跟踪 性 能 改善 时 ,a 需要 设置 
更 大 一 点 的 值 。 注 意 ,模糊 控制 推理 系统 的 输出 Q.., 和 a 是 有 界 的 : 
0 < Qc; < Qc; max 
0 < a; S a;max 
其 中 ,最 大 值 是 根据 先前 没有 应 用 模糊 系统 进行 试验 设置 的 。 控 制 力 和 跟踪 误差 的 变化 对 
Qo fis I Rn] nfl 2-45(a) 所 示 。 可 以 看 出 ,通常 : 当 跟踪 误差 高 且 控 制 力 低 时 ,增益 增 
加 ; 当 跟 踪 误差 低 上 且 控制 力 高 时 ,增益 最 小 。cx 的 模糊 推理 表面 如 图 2-45(b) 所 示 , 可 以 看 
出 , 当 跟 踪 误 差 低 且 控制 力 高 时 ,遗忘 因子 处 于 最 大 值 。 


表 2-8 EFE ë, 7, .二 ,模糊 隶属 度 函 数 的 推理 输出 0,., 的 真 值 表 


(2.136) 


E] 入 输 出 
规则 1 如 果 EjrEr<o 那么 Qi 小 
规则 2 如 果 Ej E0 且 ze 一 c 那么 Qc) 
规则 3 如 果 且 [20 9957 那么 Q.., 中 等 
规则 4 如 果 EjrEz>o 且 [2B 那么 Qo X 
规则 5 如 果 且 Ta ,下 ,> 那么 Q..， 中 等 
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表 2-9 EFE, EF, .F, 模 糊 录 属 度 本数 的 推理 输出 a 的 真 值 表 


输 入 输 出 
规则 1 如 果 Ej. H [20 那么 a 中 等 
规则 2 如 果 且 [2020 那么 aX 
规则 3 如 果 Ej E0 且 [2p 那么 a 中 等 
规则 4 如 果 且 r.PF.s 那么 a 大 
规则 5 如 果 EjrEs>o 且 zuo Fx 那么 a 小 
规则 6 如 果 且 Dy 那么 aco PE 


(a) Cft d E if (b) 以 es 和 所 作为 输入 
图 2-45 规则 表面 (任务 空间 中 的 增益 的 表面 也 具有 类 似 特性 ) 


3. 稳定 性 
控制 器 在 关节 空间 中 的 稳定 性 和 收敛 到 小 有 界 集合 ,并 且 笛 卡 儿 空 间 控制 器 的 证 明 与 


之 类 似 。 然 而 ,本 节 内 容 中 必须 考虑 到 的 是 ,对 角 适 应 增益 矩阵 Q..) 是 时 变 的 。 
ov. = 二 | RoRo Ro K*(s—0Qi G— òt) KG—9024 


tr(DT(o)Qa G) DG) — D' (i — 80 Qi G—2a0 DG-—80)3 

TOQ CTC) — T" (6 — dQ G —802G — st) } (2.137) 
定义 一 个 新 的 变量 oQ-—diag[ IG90Qx ,1@6Qb . 1690Q. ] Ot POE 9E 7 VJ SE BO A fe p E I 
式 的 最 后 添加 另 一 项 ,得 到 : 


= 
6V.(1) = Lf 


BT (NR C038(a)do— | 


D 


YQ GO! (o) Bo) do + 
tòt 


tòt 


| e" (o) 开 (c)e(o) 十 sr(o) DG) êlo) +eT(o) DG») êlo) + 


tòt 


OOTES $' G08Q^ G) Bg) do (2.138) 
tôt 


最 后 一 个 被 积 式 中 的 项 可 以 用 ead RR. Ep 


cad à > (ek KT R +en D" D--e£ 72), eq = max(esepse) — (2.139) 
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考虑 到 KT@r! K<ex KT K.D'os! DLev D” DR O-n Le TTT JEP eco.) IE US 
xp,: 的 最 小 特征 值 。 这 可 以 被 添加 到 公式 中 的 条 件 . 并 给 出 不 等 式 : 
V> AL |l ell? + Yslel*—» el I9* I 


zAlel*-xl$l*—» el lel zo (2.140) 
其 中 ,y =Q yy =y 十 ea。 这 是 证 明 稳定 性 的 充分 条 件 , 并 且 假 定 Q(W) 由 前 面 的 公式 中 
的 模糊 推理 的 输出 界定 。 
2.5.4 仿真 
任务 包括 跟踪 沿 y 坐标 定义 的 平滑 最 小 颠 艇 轨迹 ; 
y'= y (00-c-G'OD—y'(O»(O07 —157* 4-675), r=4 (2.141) 
其 中 本 是 运动 周期 。 关 节 空 间 角速度 通过 使 用 雅 可 比 的 伪 逆 JOSJ (E ARR: 
q'XO) = J Loy" (.0,0,0,0]" (2.142) 
从 中 可 以 分 别 求 出 位 置 和 加 速度 
«o - [à coa. q = xo (2. 143) 


使 用 具有 运动 学 和 动态 Baxter 机 器 人 刚性 关节 模型 的 MATLAB 和 Peter Corke's 
RoboticToolbox 实现 所 提出 的 任务 和 控制 器 仿真 。 为 了 在 连续 不 同 的 条 件 下 测试 控制 器 ， 
两 个 扰动 力 Fw 和 Fw 将 在 不 同 阶段 被 引入 : 

。 REI: 没有 扰动 ; 

。 KEI: 只 有 Fuss 

* 阶段 四: 只 有 Fas: 

。 阶段 N : Fua I Fasz 

对 于 每 个 阶段 分 析 性 能 ,观察 控制 器 对 不 同 扰动 的 反应 。 仿 真 期 望 看 到 的 结果 是 关节 
空间 控制 能 够 改善 对 Fe 产生 的 扰动 的 抑制 以 及 任务 空间 控制 能 够 抵抗 Fw 产生 的 扰动 。 
设置 阶段 的 顺序 是 为 了 使 读者 更 容易 理解 适应 的 整个 过 程 。 性 能 指数 了 可 以 通过 对 输入 力 
F, 和 任务 空间 跟踪 误差 6; 的 加 权 和 积分 计算 得 到 : 


7= [EOTF + d Res (Dd (2.144) 
其 中 Q.R € R: JE EXT ftt 45 JOB P «t, FL c, 被 设置 以 获得 仿真 过 程 中 每 个 阶段 的 7。 小 的 
性 能 指数 了 对 应 于 小 的 跟踪 误差 和 控制 力 , 因 此 意味 着 好 的 控制 性 能 。 
2.5.5 实验 设计 
下 面 对 Simulink 仿真 中 重要 的 框图 进行 介绍 。 


(1) 笛 卡 儿 空间 的 自 适 应 控制 模块 如 图 2-46 所 示 。 
(2) 关节 空间 的 自 适应 控制 模块 如 图 2-47 所 示 。 
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图 2-47 关节 空间 的 自 适应 控制 Simulink 框图 
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G) il 


E 向 运动 学 模块 如 图 2-48 所 示 。 


图 2-48 正 向 运动 学 Simulink 框图 


(4) 模糊 控制 的 反馈 模块 如 图 2-49 所 示 。 


2.5. 
1. 混 


图 2-49 模块 控制 Simulink 框图 


6 实验 与 结果 
合 控制 


当 控制 器 输出 力矩 分 别 满足 MOLISOREOLENOLEISORENOII EE 


2 


站 十 t(D) 十 Qrj (1) 分 别 仅 与 关节 空间 控制 器 和 任务 空间 控制 器 的 控制 性 能 进行 


比较 。 干 扰 参 数 在 每 种 情况 下 保持 不 变 ; 式 (2. 145) 中 定义 的 Fuat E p= 二 20sin(2x507)， 
式 (2.111) 中 定义 的 参数 满足 r= 二 100sin(2x0. 1042:)。 轨 迹 周 期 和 行进 距离 分 别 设 置 为 
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4. 8s 和 0.2m。 每 个 仿真 阶段 对 应 于 完成 一 次 式 (2. 141) 的 轨迹 跟踪 。 图 2-50(a) 中 有 3 个 
控制 方案 的 笛 卡 儿 跟 踪 误 差 示 出 了 当 一 个 工具 类 型 扰动 作用 于 机 械 臂 时 任务 空间 控制 器 如 
何 执 行 得 更 好 ,但 当 大 的 干扰 被 施加 在 远离 末端 执行 器 时 ,任务 空间 控制 受到 影响 。 在 这 种 
情况 下 ,关节 空间 控制 能 够 更 有 效 地 减少 跟踪 误差 。 当 混合 控制 器 将 两 者 组 合 时 ,跟踪 误差 
进一步 降低 。 从 图 2-50(b) 可 以 注意 到 ,应 用 三 种 控制 方案 所 需 的 控制 力 总 量 之 间 几 乎 没 
有 差别 。 通 过 测量 跟踪 误差 和 控制 力 , 组 合计 算出 的 每 个 阶段 的 性 能 指数 了 如 图 2-50(c) 
所 示 。 在 阶段 和 阶段 时 中 可 以 明显 看 出 的 任务 空间 和 关节 空间 控制 器 的 性 能 差异 ,其 中 
干扰 类 型 从 Fu 切换 到 Fass 任务 空间 控制 能 更 好 地 处 理 前 者 Fus 类 型 的 干扰 ,而 关节 空 
间 控 制 则 能 更 好 地 处 理 后 者 Fe 类 型 的 干扰 。 混 合 控制 器 在 阶段 下 中 关节 空间 稍微 有 所 改 
36 ,但 在 其 阶段 耻 和 阶段 W 的 组 件 部 分 则 能 够 看 出 有 所 改进 。 考 虑 到 三 种 方案 中 的 上 rm | 
类 似 , 如 图 2-50(b) 所 示 , 这 表明 混合 控制 器 以 更 有 针对 性 的 方式 应 用 控制 。 
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Phase 
(c) 在 每 个 仿真 阶段 计算 性 能 指数 7 
图 2-50 控制 器 性 能 对 比 
对 于 图 2-50(a) : 阶段 上 中 (0 二 1<4.8) ,在 跟踪 误差 方面 ,三 种 控制 器 性 能 上 没有 很 
大 的 差别 。 在 阶段 下 中 ,任务 空间 的 跟踪 误差 最 小 ,在 关节 空间 中 误差 最 大 ,混合 控制 器 则 
处 于 两 者 之 间 。 在 接 下 来 的 两 个 阶段 中 (9. 6 二 + 二 19.2) ,任务 空间 控制 产生 的 误差 最 大 ,而 
混合 控制 器 显示 出 比 组 成 部 分 低 得 多 的 跟踪 误差 。 对 于 图 2-50(b): 通过 观察 输入 力矩 ， 
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可 以 看 出 三 种 控制 方案 之 间 的 微小 差异 。 对 于 图 2-50(c) : 每 个 阶段 的 性 能 指数 7 表明 了 
在 不 同 干扰 条 件 下 每 个 控制 类 型 的 限制 。 特 别 是 在 关节 控制 表现 出 色 但 任务 空间 控制 性 能 
下 降 的 阶段 有 和 阶段 人 中 ,混合 控制 显示 出 了 两 者 改进 的 性 能 。 即 仅 对 需要 附加 反馈 的 关 
节 施 加 附加 反馈 。 

图 2-51 为 学 习 获 得 的 前 馈 力矩 和 刚度 。 通 过 查看 图 2-51(a) 中 前 馈 力矩 的 变化 ,可 以 
看 到 控制 器 如 何在 阶段 四 和 阶段 人 中 增 大 力矩 来 补偿 低频 下 ow 的 干扰 ,主要 是 在 第 一 个 关 
节 ( 沿 着 oy 平面 旋转 )。 比 较 控 制 器 之 间 前 馈 力矩 的 大 小 ,显然 ,关节 空间 控制 产生 了 更 大 
的 力矩 ,而 混合 力矩 输出 施加 在 关节 1 的 幅 值 和 权重 都 比较 小 。 
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(b) 每 个 控制 器 在 每 个 阶段 中 间 的 椭圆 刚度 
图 2-51 学 习 获 得 的 前 馈 力矩 和 刚度 


笛 卡 儿 刚 度 椭圆 如 图 2-51(b) 所 示 , 对 于 每 个 控制 器 ,在 阶段 和 下 的 中 点 ,对 zy 平 
面 中 的 用 椭圆 表示 的 刚度 几何 形状 进行 对 比 。 在 任务 空间 和 混合 控制 中 ,椭圆 主要 在 对 应 
于 扰动 方向 的 zx 轴 上 伸 长 。 在 任务 空间 和 混合 控制 中 ,可 以 观察 到 刚度 从 y 方向 上 的 轻微 
取向 (由 于 沿 着 该 轴线 移动 的 轨迹 ) 变 化 到 主要 在 工 轴 上 的 更 大 的 椭圆 ,与 扰动 作用 方向 一 
致 。 然 而 在 关节 空间 控制 中 ,刚度 椭圆 的 取向 与 扰动 方向 则 不 完全 一 致 ,这 表明 在 这 种 情况 
下 反馈 力矩 被 低 效 地 应 用 。 

2. 控制 增益 模糊 推理 

通过 在 混合 控制 器 上 实现 来 检验 控制 增益 Qc RI o 的 模糊 推理 的 有 效 性 ,并 且 与 前 面 
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部 分 (其 中 控制 增益 是 固定 的 ) 获 得 的 结果 进行 比较 。 在 式 (2. 135) 中 描述 的 基线 平均 值 和 
适应 增益 的 上 限 通过 从 先前 实验 中 混合 控制 器 运行 时 收集 的 数据 计算 获得 ,它们 随后 将 用 
作 影 响 自 适应 定律 的 模糊 推理 机 的 输入 。 

模糊 调整 参数 的 性 能 见 图 2-52。 观 察 图 2-52(a) 可 以 看 到 ,阶段 中 的 跟踪 误差 有 改 
善 ,但 其 他 阶段 没有 那么 多 ,这 类 似 于 以 前 的 结果 。 然 而 ,将 结果 与 图 2-52(b) 进 行 比较 ,可 
以 看 出 ,尽管 在 前 两 个 阶段 中 控制 力矩 没有 减 小 ,但 是 在 最 后 两 个 阶段 中 显著 减 小 ; 这 不 仅 
证 明了 在 控制 力 已 经 最 小 时 ,在 线 整定 能 够 减少 跟踪 误差 ,而 且 还 减少 了 维持 良好 跟踪 所 需 
要 的 控制 力 。 这 反映 在 图 2-52(c) 中 ,其 示 出 在 所 有 扰动 阶段 中 ,通过 在 线 调整 学 习 参 数 来 
改善 总 体 性 能 指标 得 分 。 
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(c) 固定 和 模糊 参数 控制 器 之 间 的 性 能 指数 "比较 
图 2-52 模糊 调整 参数 的 性 能 


在 图 2-53(a)、(b) 中 ,比较 近 侧 关节 的 前 馈 力 矩 , 可 以 看 到 ,刚度 椭圆 随时 间 变 化 的 
形状 在 两 个 控制 器 之 间 是 类 似 的 。 然 而 .模糊 混合 控制 器 期 间 施 加 较 大 的 前 馈 转 矩 HE 
糊 整定 有 一 个 高 得 多 的 响应 幅度 ,尽管 形状 保持 不 变 。 与 图 2-53(c)、(d) 相 比 , 利 用 模糊 
调整 参数 时 ,刚度 椭圆 幅 值 减 小 。 这 表明 在 线 调节 控制 增加 了 前 馈 力 和 矩 , 同 时 牺牲 刚度 
以 减少 在 图 2-50(b) 中 观察 到 的 控制 作用 ,尽管 椭圆 的 几何 形状 保持 在 扰动 的 方向 。 在 
图 2-53(c)、(d) 中 ,混合 控制 器 的 刚度 椭圆 与 相同 的 模糊 参数 整定 的 控制 器 相 比 ,具有 更 高 
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的 幅 值 。 注 意 ,在 模糊 整定 的 情况 改 下 的 缩放 倍数 是 0.02。 在 第 二 阶段 中 的 椭圆 在 干扰 方 
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本 章 小 结 


本 章 以 5 个 先进 的 机 器 人 研究 课题 为 实例 ,介绍 了 MATLAB 机 器 人 工具 箱 在 机 器 人 
研究 中 的 应 用 。 其 中 控制 算法 中 介绍 了 神经 网 络 控制 算法 , 自 适应 控制 算法 ,触觉 识别 算 
法 ; 与 机 器 人 相关 的 设备 包括 了 MYO 手 环 , 力 触觉 传感器 Touch X。 读 者 可 根据 自己 的 
研究 课题 ,对 感 兴趣 的 实例 进入 深入 的 研究 。 


参考 文献 


[1] Huang K. Yang C. Cheng H. Object Property Identification Using Uncertain Robot Manipulator 
[C]. Chinese Conference on Pattern Recognition. Springer Singapore. 2016; 174-188. 

[2] Yang C. Wang X. Li Z, et al. Teleoperation Control Based on Combination of Wave Variable and 
Neural Networks[]]. IEEE Transactions on Systems. Man. and Cybernetics: Systems. vol. 47 2017. 

[3] Xu Y. Yang C. Liang P. et al. Development of a hybrid motion capture method using MYO armband 


第 2 章 ”MATLAB 机 器 人 工具 箱 的 应 用 |f» 197 


with application to teleoperation[ C ]. IEEE International Conference on Mechatronics and Automation. 
TEEE. 2016: 1179-1184. 

[4] Jiang Y. Yang C. Wang X, et al. Kinematics modeling of Geomagic Touch X haptic device based on 
adaptive parameter identification[ C ]. Real-time Computing and Robotics (RCAR). IEEE International 
Conference on. IEEE, 2016; 295-300. 

[5] Smith A M C, Yang C. Ma H，et al. Novel hybrid adaptive controller for manipulation in complex 
perturbation environments[]]. PloS one, 2015, 10(6) : e0129281. 

[6] Yang C. Ma H, Fu M. Advanced Technologies in Modern Robotic Applications L M]. 科学 出 版 
ik. 2016. 


第 二 篇 ”机 器 人 仿真 软件 
的 基础 与 应 用 


机 器 人 仿真 的 意义 


仿真 指 的 是 设计 实际 或 理论 物理 系统 的 模型 ,执行 模型 和 分 析 执 行 输 
出 的 过 程 。 自 20 世纪 初 以 来 ,仿真 工具 已 成 为 一 种 重要 的 研究 工具 。 最 
初 , 它 是 作为 一 个 学 术 研 究 工 具 诞生 的 。 如 全 ,仿真 工具 作为 一 个 强大 的 
工具 ,在 科学 研究 和 工程 技术 领域 提供 设计 、 规 划 、 分 析 和 决策 等 服务 。 作 
为 现代 技术 的 一 个 分 支 ,机 器 人 仿真 技术 也 是 非常 重要 的 。 实际 上 ,仿真 
工具 在 机 器 人 应 用 研究 中 起 着 非常 重要 的 作用 。 

虚拟 仿真 软件 能 够 在 虚拟 的 计算 机 环境 中 ,对 现实 的 物理 环境 进行 仿 
真 , 并 且 这 种 仿真 效果 具有 一 定 的 逼真 度 。 在 机 器 人 学 领域 中 .机 器 人 仿 
真 技术 发 挥 着 重要 的 作用 , 它 能 够 给 机 器 人 的 研究 开发 提供 一 个 安全 可 靠 
的 平台 。 例 如 ,设计 一 个 机 器 人 完成 一 个 危险 而 复杂 的 任务 。 如 果 通 过 现 
实 环境 进行 任务 测试 ,会 使 机 器 人 面临 着 较 大 的 风险 ,同时 会 造成 一 定 的 
经 济 损失 。 通 过 机 器 人 仿真 平台 进行 测试 ,不 仅 没 有 因 任 务 失败 带 来 的 风 
险 , 而 且 能 够 进行 多 次 重复 测试 ,节省 成 本 。 但 是 ,这 对 于 机 器 人 仿真 平台 
的 要 求 极 高 , 兴 须 保证 仿真 结果 具有 真实 性 与 有 效 性 ,才能 对 实际 工作 进 
行 指 导 。 

第 一 个 商业 机 器 人 仿真 软件 由 Deneb Robotics 公司 (现在 的 
Dassault/Delmia 公司 ) 和 Tecnomatix Technologies 公司 (现在 的 
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UGS/Tecnomatix 公司 ) 在 二 十 多 年 前 联合 开发 完成 。 这 款 精 心 设计 的 产品 的 关键 
目标 是 帮助 解决 设计 和 编程 机 器 人 系统 日 瘟 复 杂 的 问题 。 直 到 现在 ,仿真 仍然 是 机 
器 人 研究 领域 的 热点 ,例如 移动 机 器 人 ,是 机 器 人 的 运动 和 抓 取 规 划 中 的 一 个 重要 
工具 。 

在 这 里 ,我 们 讨论 仿真 的 作用 ,对 机 器 人 仿真 进行 了 概述 。 这 里 有 几 个 基本 的 仿真 
概念 。 首 先是 "学习 ”原理 。 仿 真 给 出 了 非常 有 效 的 方式 来 了 解 环境 对 象 的 不 同 可 能 
性 ,并 且 允 许 通过 改变 参数 来 观察 交互 的 效果 。 仿 真 中 的 可 视 化 是 仿真 中 的 另 一 个 重 
要 因素 。 仿 真 的 可 能 性 提供 了 广泛 的 选择 ,使 人 们 能 够 将 创意 方便 地 插入 到 问题 解决 
方案 中 ,可 以 在 系统 构建 之 前 获得 结果 ,并 对 结果 进行 评估 。 同 时 ,使 用 仿真 工具 可 以 
避免 损伤 和 损坏 机 器 人 ,也 可 以 避免 在 开始 生产 之 后 对 设计 做 出 的 不 兴 要 的 改变 的 风 
险 。 在 研究 中 ,仿真 软件 使 得 构建 具有 期 望 特性 的 实验 环境 成 为 可 能 。 不 同 的 因素 ,如 
复杂 性 、 现 实 性 和 特异 性 ,可 以 在 仿真 环境 中 和 逐渐 增加 。 

仿真 已 经 是 机 器 人 技术 中 的 重要 工具 ,用 于 从 产品 的 设计 和 性 能 评估 到 它们 的 设 
计 应 用 中 。 通 过 仿真 可 以 研究 不 同 级 别 的 机 器 人 系统 的 结构 .特性 和 功能 。 系 统 越 复 
杂 ,仿真 的 作用 就 越 重 要 。 仿 真 使 得 我 们 能 够 奶 方便 地 使 用 计算 复杂 的 算法 ,如 遗传 算 
法 ,而 这 些 算 法 在 真实 的 机 器 人 微 控 制 器 上 运行 是 一 个 很 耗 时 的 过 程 。 因 此 ,仿真 可 以 
提升 机 器 人 系统 的 设计 、 开 发 甚至 操作 等 方面 的 能 力 ,根据 具 体 应 用 .不 同 的 结构 属性 
和 功能 参数 进行 建 模 。 目 前 ,市 面 上 出 现 了 各 种 仿真 工具 ,它们 广泛 地 应 用 于 机 器 人 机 
械 手 的 建 模 、 控 制 系统 和 设计、 离线 编程 系统 以 及 其 他 领域 。 

大 多 数 现 有 的 仿真 软件 主要 集中 在 机 器 人 在 不 同 环境 中 的 运动 仿真 。 所 有 仿真 系 
统 的 中 心 问题 是 如 何 对 机 器 人 的 运动 学 模型 和 动态 模型 进行 运动 仿真 。 例 如 ,轨迹 规 
划算 法 依赖 于 运动 学 模型 ; 驱动 器 的 设计 需要 动态 模型 。 因 此 ,具体 模型 的 选择 取决 于 
仿真 系统 的 目标 。 为 了 对 机 器 人 进行 建 模 和 仿真 ,可 以 采用 不 同 的 方法 。 其 中 一 个 差 
异 可 能 是 用 户 构 建 模型 的 方式 。 例 如 ,在 框图 定向 的 仿真 软件 中 ,用 户 通过 组 合 不 同 的 
块 来 创建 模型 。 替 代 方 案 是 需要 手动 编码 的 包 。 用 于 机 器 人 系统 的 仿真 工具 大 致 可 以 
分 为 2 个 组 : 

。 基于 一 般 仿真 系统 的 工具 ; 

。 机 器 人 系统 的 专用 工具 。 

第 一 组 包括 简化 机 器 人 系统 及 其 在 一 般 仿 真 系统 中 的 环境 的 实现 的 特殊 模块 。 这 
种 集成 的 工具 箱 使 得 能 够 使 用 一 般 系统 的 其 他 工具 来 实现 不 同 的 任务 。 

第 二 组 涵盖 更 具体 的 目的 ,如 离线 编程 或 机 械 设 和 计 。 特 殊 的 仿真 工具 也 可 以 专门 
用 于 某 些 类 型 的 机 器 人 ,如 移动 或 水 下 机 器 人 。 

目前 ,机 器 人 的 3D 环境 构建 以 及 建 模 是 机 器 人 仿真 软件 中 最 流行 的 应 用 程序 。 现 
代 3D 仿真 软件 包括 用 于 生成 模仿 机 器 人 更 真实 的 运动 的 物理 引擎 。 机 器 人 可 以 配备 
大 量 的 传感器 和 执行 器 ,现代 仿真 软件 还 提供 各 种 脚本 接口 。 应 用 比较 广泛 的 脚本 语 
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7 & URBI,MATLAB 和 Python。 目 前 也 存在 多 种 针对 移动 机 器 人 导航 的 仿真 软件 。 
新 算法 的 开发 和 测试 ,环境 和 机 器 人 的 建 模 , 包 括 不 同类 型 的 传感器 和 驱动 器 是 操纵 和 
抓 取 仿真 软件 兴 须 应 对 的 关键 方面 。 


机 器 人 仿真 软件 的 功能 


机 器 人 仿真 软件 用 于 为 机 器 人 创建 岩 入 式 应 用 程序 ,而 不 是 在 实际 机 器 人 上 的 物 
理 建 模 ,因此 节省 了 成 本 和 时 间 。 在 某 些 情况 下 ,这 些 应 用 程序 可 以 在 真实 机 器 人 上 传 
送 ( 或 重建 ) 而 无 须 修 改 。 机 器 人 仿真 软件 可 以 指 几 种 不 同 的 机 器 人 仿真 应 用 。 例 如 ， 
在 移动 机 器 人 应 用 中 ,基于 行为 的 机 器 人 仿真 软件 允许 用 户 创建 刚性 对 象 和 光源 的 简 
单 世界 ,编程 机 器 人 与 这 些 构 建 的 世界 交互 。 此 外 ,基于 行为 的 仿真 软件 可 以 对 有 能 力 
的 人 进行 "学习 ”, 展 示人 形态 的 人 形 质 量 。 

机 器 人 仿真 软件 最 受 欢迎 的 应 用 之 一 是 用 于 机 器 人 及 其 环境 的 3D 建 模 和 演 染 。 
这 种 类 型 的 机 器 人 软件 具有 作为 虚拟 机 器 人 的 仿真 软件 , 它 能 够 仿真 真实 工作 包 络 
中 的 实际 机 器 人 的 运动 。 一 些 机 器 人 仿真 软件 使 用 物理 引 蒙 用 于 机 器 人 的 更 真实 的 
运动 生成 。 我 们 强烈 建议 使 用 机 器 人 仿真 软件 开发 机 器 人 控制 程序 ,而 不 用 考 虎 实 
际 的 机 器 人 是 否 可 用 。 仿 真 软件 允许 机 器 人 程序 写 入 副本 并 与 在 实际 机 器 人 上 测试 
的 程序 的 最 终 版 本 离线 联合 调试 。 当 然 , 这 个 关键 点 仅 这 用 于 工业 机 器 人 应 用 ,因为 
取决 于 机 器 人 的 类 似 环 境 的 离线 编程 的 成 功 与 否 与 仿真 环境 相关 。 基 于 传感器 的 机 
器 人 动作 更 难以 仿真 或 离线 编程 ,因为 机 器 人 运动 取决 于 真实 世界 中 的 瞬时 传感器 

一 般 来 说 ,机 器 人 仿真 软件 可 以 通过 三 维 建 模 创建 虚拟 环境 中 的 机 器 人 及 其 工作 
环境 ,而 不 依 顿 于 实际 的 机 器 人 ,而 且 它 还 可 以 提供 多 种 不 同 的 机 器 人 仿真 应 用 。 在 某 
些 情况 下 ,这 些 仿真 软件 中 的 程序 可 以 通过 直接 转移 或 重建 到 实际 的 机 器 人 。 使 用 机 
器 人 仿真 软件 开发 机 器 人 控制 程序 有 许多 好 处 ,仿真 软件 多 许 人 们 在 将 程序 写 入 到 真 
实 的 机 器 人 之 前 ,多 次 地 测试 与 调试 。 

机 器 人 仿真 平台 主要 有 以 下 4 种 功能 : 

(1) 通过 本 身 仿 真 软件 或 外 部 工具 快速 地 设计 机 器 人 原型 ; 

(2) 使 用 物理 引擎 仿真 现实 运动 ,例如 使 虚拟 环境 具有 跟 现实 一 样 的 重力 条 件 ; 

(3) i& fL 65 3D 演 染 ,使 用 3D 建 模 工具 或 外 部 工具 来 构建 环境 ,例如 给 机 器 人 工作 
环境 增加 摄像 头等 功能 ; 

(4) 使 用 脚本 语言 使 机 器 人 动态 地 进行 运动 仿真 。 一 般 来 说 ,这 些 脚 本 语言 有 C. 
C++ , MATLAB,Python,Java,Perl, URBI 等 。 
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机 器 人 仿真 软件 的 种 类 及 选用 


目前 ,科研 或 商业 领域 都 存在 着 多 款 开 源 或 不 开源 的 机 器 人 仿真 软件 ,例如 V- 
REP,Gazebo, MORSE ,OpenHRP, RoboDK, SimSpark, ARS, Webots 等 。 不 同 的 机 
器 人 仿真 软件 适用 于 不 同 的 仿真 任务 ,采用 的 编程 语言 也 不 尽 相 同 。 下 表 对 不 同 的 机 
器 人 仿真 软件 进行 了 对 上 比 。 


八 种 机 器 人 仿真 软件 的 比较 
机 器 人 i 3D 泻 染 | 支持 的 CAD lx 
l waag Joam S 、 | 平台 支持 | 一 | 外 部 API 
仿真 软件 引擎 模型 格式 程 语言 
ODE/Bullet/ OBI. STL.| . C/C++ , Python, 
-InUXy 
Vortex/ DXF, 3DS. N Java, Urbi, 
V-REP MacOS X, |LUA 
Newton 内 部 | 内 部 Collada, = MATLAB. 
Windows 
Dynamics URDF Octave 
ODE/Bullet/ Linux, 
Gazebo | Simbody/ 内 部 | OGRE |SDF/URDF |Macos X, |C [C+ 
DART Windows 
MORSE | Bullet biai emer lae Diam ES a ipu: | yt 
SE e L t t 
ulle ender 游戏 引擎 MacOS X ython |Python 
Linux, C/C , Python, 
OpenHRP | ODE/ 内 部 内 部 | Java3D || VRML CER Ce Tat 
Windows Java 
STEP, Linux, 
—— | WE | opencL | GEs， MacOS X,| us C/C++ Python, 
BP. || eei | emi, Windows. | "^" MATLAB 
WRML | Android 
Linux, 
. - Ruby 的 场 | 70X C 
SimSpark | ODE 无 “| 内 部 MacOS X. Network (sexpr) 
景 图 : Ruby 
Windows 
Linux, 
ARS | ODE 无 |vTK | 无 MacOS X. |Python | 无 
Windows 
Linux, o 
Webos | 定制 的 ODE | 内 部 | GRE |" PT Mae Re |o Phon 
3 E ;RE acOS X, 
P VRML i Java, MATLAB 
Windows 


机 器 人 仿真 软件 的 选用 需要 从 物理 逼真 度 (物理 环境 的 条 件 、 特 征 逼 近 真 实 环境 的 
程度 )、 功 能 逼真 度 ( 软 件 仿真 进行 时 机 器 人 的 行为 逼近 实际 情况 中 机 器 人 执行 任务 的 
程度 )、 编 程 语言 及 扩展 性 .平台 支持 、 质 用 成 本 和 开发 成 本 等 多 方面 去 选择 。 
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物理 引擎 


物理 引擎 是 计算 机 软件 ,提供 了 在 计算 机 图 形 学 ,视频 游戏 和 电影 领域 中 使 用 的 某 
些 物理 系统 的 近似 仿真 ,例如 刚体 动力 学 (包括 碰撞 检测 )、 软 体 动力 学 和 流体 动力 学 。 
它们 的 主要 用 途 是 在 视频 游戏 (通常 作为 中 间 件 ) 时 ,用 于 实时 的 仿真 。 该 术语 有 时 更 
常用 于 描述 用 于 仿真 物理 现象 的 任何 软件 系统 ,例如 高 性 能 科学 仿真 。 

物理 引擎 通常 有 两 类 : 高 精度 物理 引擎 和 实时 物理 引擎 。 高 精度 物理 引 葡 需要 更 
多 的 处 理 能 力 来 计算 非常 精确 的 物理 模型 ,通常 被 科学 家 和 计算 机 动画 电影 使 用 。 实 
时 物理 引擎 用 于 视频 游戏 和 其 他 形式 的 交互 式 计算 , 它 使 用 简化 的 计算 和 较 低 的 准确 
性 来 及 时 计算 游戏 ,并 以 适当 的 速度 响应 游戏 。 

自 20 世纪 80 年 代 以 来 ,物理 引擎 通常 用 于 超级 计算 机 ,以 计算 流体 动力 学 建 模 ， 
其 中 粒子 被 分 配 、 组 合 以 显示 笃 环 的 力 矢 量 。 由 于 对 速度 和 精度 的 高 要 求 , 开 发 了 称 为 
矢量 处 理 器 的 特殊 计算 机 处 理 器 以 加 速 计 算 。 这 些 技 术 可 用 于 仿真 天 气 预 报 中 的 天 气 
模式 ,用 于 设计 空气 和 水 运 工 具 的 风 洞 数 据 或 包括 跑道 的 机 动车 辆 ,以 及 用 于 改进 散热 
器 的 计算 机 处 理 器 的 热 冷 却 。 与 计算 中 的 许多 计算 负载 过 程 一 样 , 仿 真 的 精确 度 与 仿 
真 的 分 辨 率 和 计算 的 精度 有 关 ; 在 仿真 中 未 建 模 的 小 波动 可 以 极 大 地 改变 预测 结果 。 

但 是 物理 引擎 也 具有 一 定 的 限制 。 物 理 引 繁 现 实 性 的 主要 限制 是 表示 作用 于 对 象 
的 位 置 和 力 的 数字 的 精度 。 当 精度 太 低 时 ,伟人 入 误差 影响 结果 ,并 且 未 在 仿真 中 建 模 的 
小 波动 可 以 显著 地 改变 预测 结果 ; 仿真 对 象 可 能 会 意外 运行 或 到 达 错 误 的 位 置 。 在 两 
个 自由 移动 对 象 以 大 于 物理 引擎 可 以 计算 的 精度 拟 合 在 一 起 的 情况 下 ,将 使 错误 复杂 
化 ,这 可 能 导致 物体 中 不 自然 的 积聚 能 量 , 这 是 由 于 开始 剧烈 振动 并 最 终 将 物体 吹 开 的 
圆 角 误 差 。 任 何 类 型 的 自由 移动 的 复合 物理 学 对 象 可 以 证 明 这 个 问题 ,但 是 它 特 别 容 
易 在 高 张力 下 和 具有 主动 物理 支承 表面 的 轮 式 物体 上 影响 链 节 。 一 般 较 高 的 精度 可 以 
降低 位 置 / 力 误差 ,但 是 是 以 计算 所 需 的 较 大 CPU 功率 为 代价 的 。 

本 书 涉 及 的 物理 引擎 包括 Bullet physics library, Open Dynamics Engine 
(ODE) ,Vortex Dynamics, Newton Dynamics, Simbody 和 DART。 它 们 在 各 自 的 网 
站 中 对 自己 的 介绍 如 下 : 

Bullet physics library 是 一 个 跨 平 台 的 开源 物理 引擎 ,支持 3D 碰撞 检测 、 刚 体 动 
力学 、 柔 体 动力 学 ,多 用 于 游戏 开发 和 电影 制作 中 。 它 支持 对 刚体 和 条 体 的 仿真 ,其 中 
支持 的 刚体 有 球体 .长 方 体 . 圆 柱 、 圆 锥 等 ,和 柔 体 包括 布料 、 绳 索 等 可 变形 对 象 。 

Open Dynamics Engine(ODE) 是 一 个 物理 引擎 ,包含 两 部 分 : 一 个 刚体 动力 学 仿 
真 引 擎 和 一 个 碰撞 检测 引擎 。 它 支持 的 几何 体 有 球体 、 圆 柱 体 、 长 方 体 、 三 角 体 、 高 度 
图 。 它 具有 先进 的 关节 类 型 和 集成 碰撞 检测 与 摩擦 。ODE 可 用 于 仿真 车 辆 ,虚拟 现实 
环境 中 的 对 象 和 虚拟 动物 ,目前 用 于 许多 电脑 游戏 .3D 创作 工具 和 仿真 工具 。 
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Vortex Dynamics; 是 一 个 不 开源 的 商业 物理 引擎 ,可 制作 高 精度 的 物理 仿真 。 它 
为 实现 大 量 的 物理 性 质 提供 了 与 真实 世界 相同 的 参数 ,使 得 这 个 引擎 更 真实 和 准确 。 

Newton Dynamics; 是 一 个 跨 平 台 的 物理 仿真 库 。 它 实现 了 一 个 确定 的 解 算 器 ， 
不 是 基于 传统 的 适 代 方法 或 连接 控制 ,但 同时 具有 稳定 性 和 快速 性 。 这 个 特征 使 得 它 
不 但 支持 游戏 引擎 ,而 且 可 支持 实时 的 物理 仿真 。 

Simbody 可 用 于 内 部 坐标 和 粗 粒度 分 子 建 模 ,大 型 机 械 模型 如 骨骼 ,以 及 任何 其 他 
可 以 建 模 为 通过 关节 互 连 , 由 力作 用 和 受 约 束 限 制 的 任何 东西 。Simbody 是 一 个 
SimTK 工具 集 ,提供 通用 多 体 动力 学 能 力 , 即 在 任意 约束 的 任何 一 组 广义 坐标 中 解决 
牛顿 第 二 定律 (下 =7l) 的 能 力 。Simbody 作为 一 个 开源 、 面 向 对 象 的 C++ API 提供 的 
物理 引擎 , 它 能 够 用 于 高 性 能 、 精 度 控制 的 科学 工程 。 因 此 , 它 适用 于 大 型 机 械 模 型 ,如 
人 类 步 态 、 机 器 人 ,化 身 和 动画 的 神经 肌肉 模型 。Simbody 也 可 以 用 于 实时 交互 式 应 用 
程序 的 生物 仿制 以 及 虚拟 世界 和 游戏 。 

DART( 动 态 动 画 和 机 器 人 工具 包 ) 是 由 Georgia Tech Graphics Lab 和 
Humanoid Robotics Lab 创建 的 协作 的 跨 平 台 开 源 库 。 该 库 为 机 器 人 和 计算 机 动画 中 
的 运动 学 和 动态 应 用 程序 提供 数据 结构 和 算法 。DART 的 突出 之 处 在 于 其 精确 性 和 稳 
定性 ,因为 它 使 用 广义 坐标 来 表示 镜 接 的 刚体 系统 ,而 Featherstone bh ti jktk tR 
计算 运动 的 动力 学 。 对 于 开发 人 员 来 说 ,与 许多 将 仿真 器 视 为 黑 盒 的 流行 物理 引擎 相 
反 ,DART 可 以 完全 访问 内 部 运动 和 动态 量 , 例 如 质量 矩阵 、 科 里 奥 利 和 离心 力 、 变 换 矩 
阵 及 其 导数 。DART 还 为 任意 身体 点 和 坐标 系 提供 了 和 雅 可 比 和 矩阵 的 有 效 计 算 。DART 
的 帧 语义 多 许 用 户 定义 任意 参考 帧 (惯性 和 非 惯 性 ) ,并 使 用 这 些 帧 来 指定 或 请 求 数据 。 
对 于 气 密 代码 安全 性 ,通过 情 性 评估 自动 更 新 正 向 运动 和 动态 值 ,使 DART 适合 实时 控 
制 器 。 此 外 ,DART 提供 了 扩展 API 以 便 将 用 户 提供 的 类 说 入 到 DART. 数据 结构 中 。 
接触 和 碰撞 使 用 隐 式 时 间 步 进 算 法 ,基于 速度 的 LCP( 线 性 互补 问题 ) 来 处 理 , 以 确保 非 
穿 透 ,定向 摩 掠 和 近似 的 库仑 摩擦 锥 条 件 得 到 满足 。DART 在 机 器 人 和 计算 机 动画 中 
具有 广泛 应 用 ,因为 它 具 有 多 体 动 态 仿真 器 和 用 于 控制 和 运动 规划 的 各 种 运动 工具 。 


参考 链接 


以 下 网 址 可 以 提供 一 些 参 考 : 

(1) https://en.wikipedia.org/wiki/Robotics_simulator 
(2) https://en.wikipedia.org/wiki/Physics_engine 

(3) https://en. wikipedia.org/wiki/Open Dynamics Engine 
(4) http; //butletphysics. org/ wordpress / 

(5) http. //www.ode.org/ 

(8) http; //newtondynamics. com/ forum/newton. php 

(7) https;//simtk.org/projects/simbody/ 

(8) https; //en. wikipedia.org/wiki/Vortex (software) 

(9) https; //dartsim.github. io/ 


V-REP 在 机 器 人 仿真 中 的 应 用 


3.1 V-REP 简介 及 安装 
3.1.1 V-REP 的 简介 


V-REP (英文 全 称 为 Virtual Robot Experimentation Platform) 是 一 款 开 源 的 机 器 人 仿 
真 软件 , 它 可 以 创建 、 组 成 虚拟 机 器 人 并 进行 仿真 。 因 为 V-REP 具有 多 种 类 型 的 功能 与 特 
性 ,丰富 的 应 用 编程 接口 ,所 以 也 被 称 为 机 器 人 仿真 器 中 的 “瑞士 军刀 ”。 作 为 一 款 机 器 人 仿 
真 软件 ,V-REP 可 用 于 快速 算法 开发 .工厂 自动 化 仿真 .快速 原型 设计 和 验证 .与 机 器 人 相 
关 的 教学 .远程 监控 .产品 的 安全 检测 等 任务 。 


3.1.2 V-REP 的 特性 


V-REP 有 以 下 几 种 主要 的 特性 : 

(D. V-REP 支持 Windows, MacOS, Linux 三 种 操作 系统 。 

(2) V-REP 使 用 集成 开发 环境 ,并 基于 分 布 式 控制 体系 结构 。 因 此 ,每 个 对 象 /模型 可 
以 通过 艇 入 式 脚本 插件 .附加 组 件 、.ROS 节点 、 远 程 客户 端 应 用 编程 接口 或 自 定义 的 解决 
方案 ,单独 地 被 控制 。 

CD 控制 器 可 用 C/C++ 、Python、Java、Lua、MATLAB、Octave 或 者 Urbi 等 编程 语言 编 
写 。 

(4) V-REP 中 具有 4 个 物理 引擎 : Bullet physics library, Open Dynamics Engine 
(ODE) „Vortex Dynamics 和 Newton Dynamics。 其 中 Bullet 引擎 提供 了 Bullet 2. 78 和 
Bullet 2. 83 两 个 版 本 的 物理 引擎 。 

(5) V-REP 具有 五 种 主要 的 计算 模块 : 正 向 和 逆向 运动 学 模块 ,动力 学 或 物理 模块 .路 
径 规划 模块 、 碰 撞 检 测 模块 .最 小 距离 计算 模块 。 


3.1.3 V-REP 的 安装 
本 书 选择 了 Pro-edu 中 的 Windows 版 本 ,将 安装 包 下 载 至 本 地 电脑 中 。 双 击 安装 程序 
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可 进入 安装 界面 。V-REP 的 安装 步骤 相对 简单 .只 需 按 照 它 的 提示 ,选择 Yes 或 Next 即 可 
完成 安装 过 程 。 

一 般 来 说 ,安装 步骤 对 应 于 以 下 界面 : Welcome 界面 ( 见 图 3-1) ,License Agreement 界 
lij. Set Program Shortcuts 界面 ,Confirm Setup Settings 界面 ,Copying Files 界面 ,Setup 
Complete 界面 。 


Welcome 


Welcome to V-REP PRO EDU Setup program. This program will 
install V-REP PRO EDU on your computer. 


Itis strongly recommended that you exit Windows programs before. 
running this Setup program. 


Ckck Cancel to quit Setup and then dose any program you have 
running. Clck Next to continue with this Setup program. 


WARNING: This program is protected by copyright law and 
international treaties. Unauthorized reproduction or dstrbuton of 
tis program, or any portion of it, may result in severe cvi and 
rmnal penalties, and wil be prosecuted to the maximum extent 


«gk | Net» | | Qe 


图 3-1 V-REP 的 安装 界面 一 一 Welcome 界面 


这 里 ,需要 注意 以 下 三 点 : 

COD. 如 果 计算 机 本 身 已 安装 V-REP。 在 重新 安装 新 版 本 的 V-REP 时 , 它 会 提示 “是 否 
印 载 掉 已 安装 的 V-REP”, 再 进行 重 装 。 

(2) 安装 V-REP 的 默认 路 径 为 C:\Program Files (x86)\V-REP3\V-REP_PRO_ 
EDU ,用 户 无 法 进行 更 改 。 

(3) 安装 完毕 后 ,会 弹出 对 话 窗口 ,询问 是 否 要 安装 Visual Studio 2010。 如 果 计 算 机 
本 身 没 有 Visual Studio 2010 ,应 按照 它 的 提示 单 击 Yes, 让 系统 自动 安装 。 接 下 来 , 它 会 提 
示 是 否 对 Microsoft Visual C++ 2010 x86 Redistributable HIF ÉA RHR. DE AR Hi 
用 户 计算 机 本 身 的 需求 进行 合理 的 选择 。 


3.2 V-REP 的 用 户 界面 及 位 姿 操 作 


启动 V-REP 应 用 程序 时 ,计算 机 CWindows 系统 ) 将 会 自动 创建 一 个 控制 台 窗口 ,随后 
将 控制 台 窗口 自动 隐藏 (这 种 默认 隐藏 控制 台 窗 口 的 配置 可 以 在 用 户 设 定 对 话 框 中 更 改 )。 
控制 台 窗 口 仅 用 于 输出 信息 ,例如 表示 插件 是 否 安装 成 功 。 正 常 启动 后 V-REP 会 初始 化 
一 个 默认 的 场景 ,如 图 3-2 所 示 ,场景 周 围 分 布 着 应 用 栏 、 菜 单 栏 .工具 栏 .模型 浏览 器 等 窗 
口 部 件 。 
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应 用 栏 菜单 栏 工具 栏 2 


+e 
RO 
E= 
a pa~ 
TT s 
工具 栏 ! i Ex 
Cee 
r 
T 天 一 
场景 层次 
结构 


模型 浏览 器 


图 3-2 V-REP 的 主 界面 


3.2.1 控制 台 窗口 


控制 台 窗口 (console window) 不 是 交互 式 的 ,如 图 3-3 所 示 , 仅 用 于 输出 信息 ,其 功能 
是 显示 所 加 载 的 插件 和 告知 用 户 初始 化 过 程 是 否 成 功 。 用 户 可 以 使 用 Lua 打印 命令 (从 脚 
本 中 ) 直 接 输 出 信息 到 控制 台 窗 口 。 使 用 插件 中 的 C printf 或 std :: cout 命令 也 可 将 信息 
输出 到 控制 台 窗 口 。 除 此 之 外 ,用 户 可 以 以 编程 的 方式 创建 辅助 控制 台 窗 口 以 显示 对 实例 
进行 仿真 时 特定 的 信息 。 


图 3-3 控制 台 窗 口 


在 Windows 系统 中 : 当 V-REP 应 用 程序 启动 时 ,将 创建 一 个 控制 台 窗 口 ,但 会 再 次 直 
接 隐藏 。 隐 藏 控制 台 窗 口 的 默认 配置 可 以 在 用 户 设置 对 话 框 中 更 改 。 

在 Linux 系统 中 : V-REP 需要 从 控制 台 启 动 . 它 在 V-REP 整个 运行 的 过 程 中 保持 
可 见 。 

在 MacOSX 系统 中 : 最 好 是 从 终端 启动 V-REP., 以 使 消息 可 见 。 


3.2.2 ”对话 框 


在 应 用 程序 窗口 旁边 ,用 户 还 可 以 通过 调整 对 话 框 (dialogs) 设 置 或 参数 来 编辑 场景 与 
场景 交互 。 每 个 对 话 组 合 一 组 相关 函数 或 适用 于 同一 目标 对 象 的 函数 。 由 于 多 种 原因 , 例 
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如 对 象 被 选择 为 不 同 的 状态 ,对 话 框 的 内 容 可 能 是 上 下 文 不 相互 关联 的 ,需要 用 户 加 以 
区 分 。 


3.2.3 应 用 程序 窗口 


应 用 程序 窗口 (application window) 是 应 用 程序 的 主 窗口 。 它 用 于 显示 、 编 辑 .仿真 和 
与 场景 交互 。 当 在 应 用 程序 窗口 中 激活 时 ,鼠标 左 键 、. 鼠 标 按钮 .鼠标 滚轮 以 及 键盘 具有 特 
定 的 功能 。 在 应 用 窗口 内 ,输入 设备 (鼠标 和 键盘 ) 的 功能 将 根据 上 下 文 或 激活 位 置 而 变化 。 

1. 应 用 程序 栏 

应 用 程序 栏 (application bar) 给 出 了 以 下 几 点 信息 : V-REP 副本 的 许可 证 类 型 .当前 正 
在 显示 的 场景 的 文件 名 、 一 个 演 染 通道 (一 个 显示 通道 ) 使 用 的 时 间 和 仿真 软件 的 当前 状态 
(仿真 状态 或 当前 使 用 的 编辑 模式 类 型 )。 此 外 ,还 可 以 通过 拖 动 V-REP 相关 的 文件 到 应 
用 程序 栏 或 应 用 程序 窗口 范围 内 的 任意 位 置 ,将 文件 加 载 到 当前 的 场景 中 。 支 持 的 文件 包 
插 “* ,ttt”-files(V-REP 场景 文件 ),* .ttm” 文 件 (V-REP EU cf) RI" * .ttb”。 

2. 菜单 栏 

菜单 栏 (menu bar) 可 以 访问 V-REP 中 所 有 的 功能 。 如 图 3-4 所 示 ,其 中 File 栏目 可 用 
于 文件 的 建立 和 保存 ,或 者 对 利用 3DMAX、Soildworks 等 软件 建立 的 模型 进行 加 载 ; Edit 
栏目 主要 用 于 对 场景 对 象 进行 编辑 ; Add 栏 日 主要 用 于 在 场景 中 添加 对 象 ; Simulation 栏 
目 主要 用 于 对 V-REP 仿真 的 设置 和 控制 ; Tools 栏 日 主要 用 于 对 用 户 界面 窗口 的 启用 和 关 
闭 ; Plugin ££ H f Add-ons 栏目 分 别 与 两 种 编程 方法 有 关 : 插件 和 附加 组 件 ; 通过 Help 栏 
目 可 以 直接 访问 V-REP 自 带 的 用 户 手 册 ,版 本 信息 等 。 


Load Operéi-based custom Uls 


Close scene 


Save model as 
Save Opec&l-based custom UIs as 
Isport 
Export 


Quit 


图 3-4 菜单 栏目 及 File 栏目 展开 后 显示 的 功能 


3. 工具 栏 

工具 栏 (toolbars) 如 图 3-5 所 示 , 它 分 为 两 个 栏目 ,其 中 工具 栏 1 位 于 菜单 栏 下 方 , 工 具 
栏 2 位 于 模型 浏览 器 的 左边 。 工 具 栏 1 主要 用 于 编辑 模型 及 场景 .设置 仿真 参数 和 控制 仿 
真 过 程 。 其 中 包括 了 对 场景 视角 的 旋转 、 放 大 和 缩小 ; 对 模型 在 三 维 空间 中 的 移动 与 旋转 ; 
对 模型 进行 装配 与 拆卸 ; 控制 模型 仿真 运行 .暂停 与 结束 停止 (包括 仿真 速度 的 选择 ) 。 工 
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有 具 栏 2 包含 对 仿真 参数 的 设置 ,脚本 的 插入 与 设置 ,路 径 点 的 规划 、 计 算 模 块 的 启用 与 设置 ， 


自 定 义 用 户 界面 的 编辑 等 。 总 体 而 言 ,工具 栏 包括 了 V-REP 中 所 有 工具 功能 。 


可 视 化 
位 置 移动 ”装配 /拆卸 。 撤销 /验证 


«de e E x A) Q 9 [site 2e 


单 击 选择 “位置 旋转 ”DNA 转移 重 做 选择 物 
理 引擎 


物理 引擎 设置 开始 暂停 停止 。 减速 加 速 。 可视化 场景 选择 


LT 


rronrmTTBSE XE! 


仿真 时 间 步 长 实时 仿真 开关 “线程 演 染 页 面 选择 
(a) 工具 栏 1 


仿真 设置 ”计算 模块 WE 。 路 径 编辑 “对象 选择 ”场景 层次 录像 机 


la A ju $^ i OL LE) 7o 


对 象 性 质 ”集合 物件 ”形状 编辑 自 定义 UI 模型 浏览 ”图 层面 板 ， 用 户 设 定 
(b) 工具 栏 2 


E35 工具 栏 


4. 模型 浏览 器 

仿真 浏览 器 以 文件 夹 的 形式 呈现 ,位 于 V-REP 主 界面 的 左边 ,如 图 3-6 所 示 , 文 件 中 包 
含 了 V-REP 中 所 有 的 模型 ,选择 其 中 一 个 文件 夹 ,模型 浏览 器 的 下 方 将 会 显示 出 文件 夹 内 
所 有 模型 的 预览 缩 略 图 及 名 称 。 模 型 浏览 器 中 所 有 的 模型 都 存放 在 V-REP 安装 文件 夹 的 
相对 路 径 : ...\V-REP3\V-REP_PRO_EDU\ models 中 ,模型 文件 的 扩展 名 为 . ttm。V-REP 
默认 的 模型 包含 了 交通 工具 模型 ,人 体 模型 ,可 移动 的 机 器 人 模型 和 不 可 移动 的 机 器 人 模 


型 等 。 


使 用 这 些 程序 提供 的 模型 非常 简单 ,只 需要 用 鼠标 拖 搜 模型 的 预览 缩 略 图 到 空白 的 场 


景 中 , 场 


景 将 会 自动 加 载 相 关 的 模型 ,以 供用 户 下 一 步 的 使 用 。 其 中 ,有 些 模型 已 经 包含 了 


可 执行 的 脚本 , 单 击 “ 仿 真 ”按钮 可 以 看 到 默认 的 仿真 结果 。 
5. 场景 层次 结构 
场景 层次 结构 (scene hierarchy) 显 示 了 一 个 场景 的 所 有 内 容 , 即 场景 中 的 所 有 场景 对 


象 。 它 用 


类 似 树 的 层次 结构 显示 了 场景 中 的 每 个 元 素 ,而 这 些 元 素 都 能 够 被 展开 或 折 释 。 
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图 3-6 模型 浏览 器 


可 以 通过 鼠标 双击 一 个 场景 对 象 的 名 称 ,修改 对 象 名 称 , 修 改 之 后 单 击 Enter 按钮 进行 保 
存 。 场 景 对 象 名 称 前 面 的 图 形 代 表 了 场景 对 象 的 类 型 ,双击 这 些 图 形 可 以 打开 场景 对 象 属 
性 编辑 框 。 

如 图 3-7 所 示 ,将 d12 机 器 人 加 载 到 场景 中 ,在 场景 层次 结构 窗口 也 将 自动 加 载 它 的 层 
次 结构 。 可 以 看 出 ,dl12 机 器 人 主要 由 它 的 主体 .缓冲 器 .轮子 等 部 分 构成 ,其 中 还 添加 了 摄 
像 头 力 传感器 ,而 且 这 里 所 有 的 元 素 都 按照 一 定 的 结构 进行 排列 组 合 。 在 V-REP 中 , 场 
景 对 象 与 场景 对 象 之 间 通 过 Edit 栏 的 object parent 形成 层次 等 级 (这 种 层次 等 级 关系 的 正 
确 组 合 对 于 一 个 完整 模型 的 搭建 十 分 重要 ) 。 

EX Ta" 
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nes 
dr20_trejectory_ 
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图 3-7 dr20 模型 及 其 层次 结构 
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6. 其 他 用 户 界面 

信息 文本 在 仿真 时 将 会 被 自动 加 载 , 它 记录 了 仿真 时 的 有 关 数 据 ; 状态 栏 在 仿真 时 会 
显示 仿真 过 程 的 信息 ,如 果 相关 代码 存在 漏洞 :也 将 会 在 这 里 给 出 相关 提示 ; 弹出 窗口 会 在 
单 击 鼠标 右键 时 弹出 ,使 用 它 可 以 方便 快捷 地 添加 场景 模型 和 编辑 场景 。 

页 面 (page) : 每 个 场景 可 以 包含 最 多 8 个 页 面 , 每 个 页 面 可 以 包含 无 限 数量 的 视图 。 
页 面 可 以 看 作 是 视图 的 容器 。 

视图 (view) ; 页 面 中 可 以 包含 无 限 数量 的 视图 。 视 图 用 于 显示 可 视 对 象 (例如 相机 、 图 
形 或 视觉 传感器 ) 看 到 的 场景 (其 本 身 包含 环 境 和 对 象 ) 。 

信息 文本 (information text) : 信息 文本 显示 与 当前 对 象 /项 目 选择 以 及 仿真 运行 时 的 
状态 或 参数 相关 的 信息 。 文 本 显示 可 以 通过 页 面 左上 方 两 个 小 按钮 的 其 中 一 个 进行 切换 。 
另 一 个 按钮 可 用 于 切换 白色 背景 ,根据 场景 的 背景 颜色 提供 更 好 的 对 比 度 。 

状态 栏 (status bar): 状态 栏 显示 与 执行 的 操作 、 命 令 有 关 的 信息 ,并 显示 来 自 Lua fff 
释 器 的 错误 消息 。 在 脚本 中 ,用 户 还 可 以 使 用 simAddStatusbarMessage 函数 将 字符 串 输出 
到 状态 栏 。 状 态 栏 默认 只 显示 两 行 ,但 可 以 使 用 其 水 平分 隔 句柄 调整 大 小 。 

自 定义 用 户 界 面 (custom user interfaces); 自 定义 用 户 界面 是 可 用 于 显示 信息 (如 文 
本 、 图 像 等 ) 或 自 定义 对 话 框 的 用 户 定义 的 UI 界面 ,允许 以 自 定义 的 方式 与 用 户 交互 。 

弹出 菜单 (popup menu): 弹出 菜单 是 在 单 击 鼠 标 右键 后 出 现 的 菜单 。 要 激活 弹出 式 菜 
单 ,请 确保 鼠标 在 单 击 操作 期 间 不 移动 ,否则 可 能 会 激活 相机 旋转 模式 。 应 用 窗口 (例如 , 场 
景 层次 视图 、 页 面 、 视 图 等 ) 内 的 每 个 表面 可 以 触发 不 同 的 弹出 菜单 (注意 区 分 不 同 的 情景 ) 。 
弹出 菜单 的 内 容 也 可 以 根据 当前 的 仿真 状态 或 编辑 模式 而 改变 。 除 了 仅 在 视图 或 页 面 上 激 
活 弹 出 菜单 时 出 现 的 视图 菜单 项 之 外 ,还 可 以 通过 菜单 栏 访 问 大 多 数 弹 出 菜单 功能 。 


3.2.4 自 定义 用 户 界面 


V-REP 提供 了 两 种 不 同 的 自 定义 用 户 界面 ,一 种 是 基于 Qt 的 自 定义 用 户 界面 , 另 一 种 
是 基于 OpenGI 的 自 定义 用 户 界面 , 见 图 3-8。 

基于 Qt 的 自 定义 用 户 界面 采用 Qt 框架 提供 的 功能 。 因 为 它 是 封装 在 一 个 V-REP Hi 
件 (v_repExtCustomUI) 中 , (插件 源 代码 的 路 径 为 : …\V-REP3\V-REP_PRO_EDU\ 
programming\v_repExtCustomUTD) ,所 以 它 更 加 容易 为 用 户 扩展 自身 功能 。 这 种 界面 提供 
Qt 类 型 的 部 件 ,如 集成 了 按钮 的 对 话 框 ,编辑 框 \ 滑 块 ,标签 、 图 片 等 。 任 何在 自 定义 用 户 界 
面 上 的 动作 (例如 按钮 单 击 、 文 字 版 . 滑 块 运动 ) 都 以 一 个 回调 脚本 的 形式 进行 记录 ,其 他 的 
功能 调用 通过 相关 的 API 函数 访问 。 

基于 OpenGI 的 自 定义 用 户 界面 是 V-REP 第 一 个 开发 的 自 定义 UI 类型, 并且 基于 
OpenG1。 它 相对 灵活 ,但 是 在 开发 复杂 UI 时 可 能 是 不 切实 际 和 过 于 耗 时 的 。 基 于 OpenG1 
的 自 定义 UI 可 以 采用 集成 按钮 编辑 框 、 滑 块 或 标签 的 对 话 框 的 形式 。 对 自 定义 UI 执行 
的 任何 操作 (例如 ,按钮 单 击 .文字 编辑 、 滑 块 移动 ) 都 会 报告 为 可 被 相应 的 API 调用 拦截 的 
消息 。 这 很 大 程度 上 人 允许 自 定义 仿真 过 程 。 
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This is a deme of the CustemDI plugin Browse threugh the tabs below te esplore 
all the widgets that can be crested with the plugin. 


Buttons | Numeric  Combober | Image | Larosts 


Simple button 


Malti valued option: 


These are radiobutton elementa. Tou cas only select one of these 
Wo item has boen selseted 


Option sat 


These are checkbos elements Tow cas select mltisle cf tbese. [This is a textured button 


Selection: etr textur ET é p 


wind robot exporimentotion piatto 


图 3-8. 左 图 为 基于 Qt 的 UI 界面 , 右 图 为 基于 OpenGI ff UI 界面 


此 外 ,在 V-REP 的 官方 声明 中 宣称 ,将 来 不 可 能 进一步 发 展 或 改进 基于 OpenG1 开发 
的 用 户 自 定义 界面 。 因 此 ,在 这 里 ,我 们 建议 读者 去 学 习 使 用 基于 Qt 的 自 定义 UI。 

在 V-REP 本 身 提 供 的 场景 (V-REP3\V-REP_PRO_EDUNscenes) 中 分 别提 供 了 以 上 
两 种 自 定义 界面 的 场景 ,文件 名 为 customUI-OpenGlBased. ttt 和 customUI-QtBased. ttt, 
运行 这 两 个 文件 ,可 以 将 它们 的 界面 风格 .输入 命令 等 进行 比较 。 

3.2.5 页 面 与 视图 

V-REP 中 的 视图 用 于 显示 特定 对 象 的 图 像 内容 。 其 中 这 个 对 象 必须 是 可 被 查看 的 。 
V-REP 中 的 页 面 是 场景 主要 能 够 被 用 过 观看 的 表面 。 它 可 以 由 一 个 或 一 个 以 上 的 视图 组 
成 。 例 如 ,一 个 视图 与 相机 对 象 相关 联 ,那么 它 可 以 显示 相机 看 到 的 内 容 。 如 图 3-9 所 示 ， 


它 说 明了 页 面 、 视 图 和 可 见 对 象 之 间 的 关系 : 页 面 可 以 包含 多 个 视图 ,每 个 视图 表示 同一 只 
移动 机 器 人 的 不 同 图 像 ; 而 视图 则 显示 了 相对 应 相机 的 视角 ,与 相机 的 位 置 及 配置 有 关 。 


Ld $ 


关联 于 视角 关联 于 视角 
较 远 的 相机 较 近 的 相机 


页 面 
/ 


视图 
图 3-9 页 面 .视图 和 可 见 对 象 之 间 的 关系 


在 页 面 中 ,视图 可 以 具有 一 个 固定 位 置 , 也 可 以 浮动 于 页 面 上 , 即 可 以 根据 用 户 的 需要 
去 调动 位 置 。 
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页 面 、 视 图 和 可 见 对 象 之 间 的 关系 如 下 页 面 配 置 所 示 : V-REP 
中 的 每 个 场景 都 有 8 个 可 自由 配置 的 页 面 。 可 以 通过 页 面 选择 器 工 
具 栏 按钮 ( 见 图 3-10) 访 问 ( 即 显示 ) 单 个 页 面 。 
当 创建 新 场景 时 ,8 个 页 面 中 的 每 一 个 页 面 将 以 不 同 的 方式 被 预 。 图 3!10 页 面 选择 器 
RUE. 工具 栏 按钮 
创建 页 面 和 选择 默认 页 面 配置 时 ,依次 选择 : Popup menu( 弹 出 
菜单 ) „Set-Up Page With( 设 置 页 面 ); 
删除 某 个 页 面 时 ,依次 选择 : Popup menu( 弹 出 菜单 )、Remove Page( 删 除 页 面 )。 之 
后 ,不 存在 的 页 面 ( 即 已 删除 的 页 面 ) 将 显示 深 灰 色 表 面 。 
有 几 种 默认 页 面 配置 可 用 ,如 图 3-11 所 示 (数字 表示 视图 索引 ) 。 


[* TE 
[Ej eer L BH ki 
LER | Lel h| 
E 
Lele] Ts 


图 3-11 具有 固定 视图 的 可 用 页 面 配置 


页 面 上 方 配置 允许 显示 1 一 8 个 固定 视图 。 浅 灰色 表面 中 的 每 一 个 对 应 于 空 视图 ( 即 非 
关联 视图 ) 。 在 任何 时 候 , 可 以 依次 单 击 弹出 菜单 删除 页 面 以 删除 页 面 。 删 除 页 面 还 会 删 
除 其 包含 的 所 有 视图 ,但 不 会 删除 任何 关联 的 对 象 。 在 现 有 页 面 配置 的 顶部 ,可 以 依次 单 击 
弹出 菜单 、 添 加、 浮动 视图 以 添加 无 限 数 量 的 浮动 视图 。 

浮动 视图 可 以 移动 和 调整 大 小 .但 不 允许 使 用 左 或 右 鼠 标 按钮 进行 导航 (例如 相机 转 
换 、 相 机 旋转 等 )。 双 击 浮动 视图 以 使 用 视图 索引 以 交换 其 内 容 ( 但 是 ,可 以 通过 编程 禁用 它 
来 禁止 此 操作 )。 要 删除 浮动 视图 ,只 需 单 击 其 右上 方 的 按钮 即 可 。 要 将 可 查看 对 象 与 视图 
相关 联 , 请 选择 该 对 象 ,并 在 我 们 要 关联 的 视图 上 依次 单 击 弹 出 菜单 、 视 图、 与 所 选 摄像 机 关 
联 视图 ,以 及 弹出 菜单 、 视 图 、 关 联 视 图 与 所 选 图 形 或 依次 单 击 弹出 菜单 、 视 图 ,与 所 选 视觉 
传感器 关联 视图 (弹出 菜单 将 根据 最 后 选择 的 对 象 自动 调整 其 内 容 )。 当 创建 视图 但 尚未 与 
可 见 对 象 关 联 时 ,依次 单 击 弹出 菜单 添加、 相机 命令 将 添加 一 个 摄像 机 ,并 直接 将 其 与 视图 
相关 联 ( 即 透视 它 )。 给 定 的 可 见 对 象 可 以 与 任何 数量 的 视图 同时 关联 。 或 者 通过 在 视图 中 
激活 以 下 弹出 窗口 将 视图 与 可 见 对 象 相关 联 : 依次 单 击 弹出 菜单 、 视 图 ,视图 选择 器 。 这 将 
打开 视图 选择 器 ,如 图 3-12 所 示 。 

一 旦 观看 对 象 与 视图 相关 联 , 视 图 中 将 自动 显示 其 图 像 内 容 ,如 果 采 用 视觉 传感器 产生 
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图 3-12 查看 视图 选择 器 


其 图 像 内 容 , 则 需要 调用 适当 的 API 命令 。 默 认 情 况 下 , 主 脚本 包含 将 处 理 场 景 中 所 有 视 
觉 传 感 器 的 命令 (simHandleVisionSensor(sim_handle_all))。 除 非 视 觉 传感器 被 标记 为 显 
式 处 理 ,但 未 被 明确 地 人 处理, 否则 其 图 像 内 容 将 在 仿真 期 间 生 成 。 与 摄像 机 相关 联 的 视图 可 
以 通过 以 下 方式 进行 自 定义 或 调整 : 它 可 以 以 实体 或 线 框 显 示 摄 像 机 视图 : 依次 单 击 弹出 
菜单 .视图 .实体 浑 染 ; 它 可 以 透视 或 正 投影 显示 摄像 机 视图 模式 : 依次 单 击 弹出 菜单 、 视 
图 .透视 投影 ; 它 可 以 显示 或 隐藏 边缘 : 依次 单 击 弹出 菜单 .视图 .可见 边缘 ; 它 可 以 显示 边 
缘 为 粗 线 或 细 线 : 依次 单 击 弹出 菜单 `View Thick edges; 它 可 以 打开 和 关闭 纹理 显示 : 依 
次 单 击 弹 出 菜单 ,视图 ,形状 纹理 启用 ; 其 相关 的 摄像 机 可 以 跟踪 最 后 选择 的 对 象 : 依次 单 
击 弹出 菜单 .视图 .跟踪 所 选 对 象 。 与 图 形 相 关联 的 视图 可 以 通过 以 下 方式 进行 自 定 义 或 调 
整 : 可 以 显示 图 形 对 象 的 时 间 图 : 依次 单 击 弹出 菜单 .视图 .显示 时 间 图 ; 图 形 对 象 的 x/y 
图 形 : 依次 单 击 弹出 菜单 .视图 .显示 x/y 图 形 ; 它 可 以 自动 调整 为 图 形 内 容 : 依次 单 击 弹 
出 菜单 、 视 图、 仿真 时 自动 调整 大 小 ; 它 可 以 保持 比例 为 1 : 1: 依次 单 击 弹 出 菜单 视图、 保 
持 比 例 在 1: 1。 


3.2.6 ”对象 /项 目 位置 和 方向 操作 
在 通过 坐标 和 变换 对 话 框 修改 对 象 配置 之 后 ,还 可 以 使 用 鼠标 直接 操作 对 象 , 当选 择 


对 象 时 ,可 以 使 用 以 下 工具 栏 按钮 ( 见 图 3-13) 移 动 或 旋转 对 象 : CY 
如 图 3-14 所 示 ,默认 情况 下 ,平移 是 在 X-Y 平面 进行 的 。 Į 


取决 于 对 象 操纵 设置 中 的 设置 ,其 还 可 以 沿 着 不 同 的 轴 /平面 并 ea 
且 相对 于 每 个 单独 对 象 的 不 同 参考 系 执行 。 图 中 深 色 阴 影 覆 盖 HOS 操作 工具 栏 按 乌 
处 表示 当前 平移 平面 或 轴 。 默 认 情况 下 ,围绕 对 象 自己 的 民 轴 idol 
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执行 旋转 。 也 可 以 根据 对 象 操纵 设置 中 的 设置 ,围绕 不 同 的 轴 或 相对 于 每 个 单独 对 象 的 不 
同 参考 系 执行 。 图 中 深 色 阴影 覆盖 处 表示 当前 旋转 轴 。 
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图 3-14. O4 SOT EE RUE HEP IE «TEE rP DR CD RE Es b o P AP i v De d a 


可 以 在 用 户 设置 对 话 框 中 调整 默认 平移 和 旋转 步 长 (捕捉 ), 但 建议 分 别 保持 em FI 5° 
的 值 ,或 者 在 对 象 操作 设置 中 单独 设置 对 象 的 步 长 。 在 对 象 操作 期 间 按 下 鼠标 按钮 后 , 按 住 
shift 键 可 暂时 禁用 捕 提 。 以 类 似 的 方式 ,在 对 象 操纵 期 间 按 下 鼠标 按钮 之 后 ,通过 按 住 
Ctrl 键 可 临时 激活 可 选 的 平移 或 旋转 轴 。 

当 仿真 软件 处 于 顶点 编辑 模式 或 路 径 编辑 模式 时 ,也 可 以 使 用 对 象 /项 目 平移 工具 栏 按 
钮 。 对 于 精确 的 对 象 定位 ,建议 通过 坐标 和 变换 对 话 框 修改 对 象 的 位 置 和 方向 。 

1. 对 象 操作 设置 

可 以 依次 单 击 菜单 栏 `. 工 具 和 对 象 操作 设置 以 显示 对 象 操作 设置 。 对 话 框 的 内 容 取决 
于 最 后 选择 的 对 象 。 如 果 示 选择 对 象 , 则 对 话 框 处 于 非 活动 状态 。 对 话 框 如 图 3-15 Bras o 


Object Manipulation Settings [x | 
Mouse translations. 
Relative to: © World | Parentframe (| Own frame 
Preferredaxes: vV alongX v aongY _ dong? 
Translation step size [m] | Default =| 
Mouse rotations 
Relative to: — World (© Parentframe (©) Own frame 
Preferred axis: — aboutx [aboutY — (€) about Z 
Rotation step size [deg] Default E 
|. Mouse manipulation disabled when simulation not running 
|... Mouse manipulation disabled when simulation running. 


3-15 ”鼠标 平移 /旋转 设置 对 话 框 


216 4| 机 器 人 仿真 与 编程 技术 


1) 鼠标 平移 

相对 于 世界 / 父 框架 / 自身 框架 : 表示 鼠标 拖 动 将 与 绝对 参考 框架 对 准 ,与 父 对 象 参 考 
框架 对 准 或 与 对 象 自己 的 参考 框架 对 准 的 平面 或 线 上 平移 选 定 的 对 象 。 

优选 轴 : 沿 X/ 沿 Y/ 沿 民 轴 ,表示 鼠标 拖 动 允许 沿 着 以 上 选择 的 参考 系 的 优选 轴 平 移 
所 选 对 象 。 在 按 下 鼠标 按钮 后 , 按 下 Ctrl 键 可 以 在 操作 过 程 中 使 用 其 他 轴 。 

平移 步 长 : 使 用 鼠标 拖 动 平移 所 选 对 象 时 使 用 的 步 长 。 在 按 下 鼠标 按钮 后 按 下 Shift 
键 仍 可 在 操作 过 程 中 使 用 较 小 的 步 长 。 

2) 鼠标 旋转 

相对 于 世界 / 父 框架 /自身 框架 : 表示 鼠标 拖 动 将 围绕 绝对 参考 框架 , 父 对 象 参考 框架 
或 对 象 自己 的 参考 框架 的 轴 旋 转 所 选 对 象 。 

优选 轴 : 沿 X/ 沿 Y/ 沿 Z 轴 ,表示 鼠标 拖 史 允许 围绕 上 面 选 择 的 参考 系 的 优选 轴 旋 转 
所 选 对 象 。 在 按 下 鼠标 按钮 后 , 按 下 Ctrl 键 可 以 在 操作 过 程 中 使 用 其 他 轴 。 

旋转 步 长 : 使 用 鼠标 拖 动 旋转 所 选 对 象 时 使 用 的 步 长 。 在 按 下 鼠标 按钮 后 , 按 下 Shift 
键 仍 可 在 操作 过 程 中 使 用 较 小 的 步 长 。 

仿真 运行 /不 运行 时 禁用 鼠标 操作 : 启用 时 , 当 仿 真 运行 /不 运行 时 ,无 法 操作 所 选 
对 象 。 

2. 坐标 和 变换 对 话 杠 

当 用 户 使 用 以 下 工具 栏 按钮 之 一 切换 到 对 象 /项 目 移动 模式 时 ,坐标 和 变换 对 话 框 变 为 
可 见 。 

对 话 框 的 内 容 取决 于 所 选 的 移动 模式 (平移 或 旋转 ) 和 最 后 选择 的 对 象 或 项 目 。 如 果 未 
选择 对 象 /项 目 , 则 对 话 框 处 于 非 活动 状态 。 如 果 选 择 了 多 个 对 象 /项 目 , 则 一 些 参数 可 以 从 
最 后 选择 的 对 象 /项 目 复 制 到 其 他 所 选 对 象 /项 目 ( 应 用 于 选择 按钮 ) 。 

1) 位 置 /平移 

当选 择 对 象 转换 工具 栏 按钮 时 ,对 话 框 的 部 分 变 为 可 见 。 位 置 /平移 对 话 框 如 图 3-16 
所 示 , 有 两 个 不 同 的 部 分 。 

CD 对 象 /项 目 位 置 : 此 部 分 允许 修改 对 象 或 项 目的 坐标 。 

相对 于 世界 / 父 框架 : 指示 坐标 相对 于 绝对 参考 系 , 或 相对 于 父 参考 系 。 

X/Y/Z 坐标 : 所 选 对 象 相 对 于 指示 的 参考 框架 (世界 框架 或 父 框架 ) 的 位 置 。 

(2) 对 象 /项 目 平 移 和 位 置 缩 放 操作 : 此 部 分 允许 将 平移 和 缩放 变换 应 用 于 对 象 或 
项 目 。 
相对 于 世界 / 父 帧 /自身 帧 : 表示 变换 将 相对 于 绝对 参考 帧 ,相对 于 父 参 考 帧 ,或 相对 于 
对 象 自己 的 参考 帧 。 

沿 着 X/Y/Z 平移 : 表示 沿 着 所 指示 的 参考 帧 (世界 、 父 或 自己 的 帧 ) 的 X 轴 、 江 轴 和 2 
轴 的 期 望 的 平移 量 。 

沿 X/Y/Z 的 标 度 : 表示 沿 着 所 指示 的 参考 系 ( 世 界 或 母体 ) 的 X\Y 和 Z 轴 的 期 望 位 置 
缩放 。 
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X«oord.[m]  |40.0000€400 |  Agply!oselecton 

Y«oord.[m]  |*0.0000€400 | | Apgiytoselecton 对 象 /项 目 

Z«oord.[m] |+1.0000e+00 | | Apply toseecton 位 置 
Apply to selection 


Object / item transiation & position scaling operations. 


对 象 /项 目 
平移 和 位 置 
缩放 操作 


图 3-16 位置 /平移 对 话 框 


2) 方向 /旋转 
选择 对 象 旋转 工具 栏 按钮 时 ,对 话 框 的 该 部 分 变 为 可 见 。 方 向 /旋转 对 话 框 如 图 3-17 
所 示 , 它 有 两 个 不 同 的 部 分 。 


对 象 /项 目 
方向 


对 象 /项 目 
旋转 操作 


Mouse manipulation: once the | pid pr 他 
orthogonal rotation axes. Use the shift-key for. 


图 3-17 方向 /旋转 对 话 框 


COD 对 象 / 项 目 方向 : 此 部 分 允许 修改 对 象 或 项 目的 方向 。 
对 象 /项 目 定向 相对 于 世界 / 父 框 架 : 表示 所 指定 的 欧 拉 角度 相对 于 绝对 参考 系 或 相对 
于 父 参 考 系 。 
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Alpha/Beta/Gamma: 所 选 对 象 相 对 于 指示 参考 框架 (世界 或 父 ) 的 欧 拉 角 。 
(2) 对 象 /项 目 旋转 操作 : 此 部 分 允许 将 旋转 变换 应 用 于 对 象 或 项 目 。 
对 象 /项 旋转 操作 相对 于 世界 / 父 帧 / 自身 帧 : 表示 变换 将 相对 于 绝对 参考 帧 ,相对 于 父 
参考 帧 或 相对 于 对 象 自己 的 参考 帧 。 
围绕 X/Y/Z 轴 旋 转 : 指示 围绕 指示 的 参考 帧 (世界 、 父 或 自己 的 帧 ) 的 X.Y 和 2 轴 的 
期 望 旋转 量 。 


3.3 V-REP 的 场景 与 模型 


3.3.1 场景 与 模型 的 关系 


V-REP 中 的 主要 仿真 元 素 是 场景 与 模型 。 其 中 ,场景 的 文件 格式 为 . ttt,V-REP 提供 
的 场景 存放 在 V-REP3NV-REP PRO EDUNscenes 中 ,可 以 通过 依次 选择 File( 文 件 ) 和 
Open scene( 打 开场 景 ) 打 开 。 模 型 的 文件 格式 为 . ttm,V-REP 提供 的 模型 主要 存放 在 模型 
浏览 器 中 。 
场景 与 模型 的 关系 是 : 
CD 场景 中 包含 了 一 个 或 多 个 模型 ,而 模型 是 只 包含 了 一 种 元 素 的 场景 。 
(2) 模型 是 一 个 场景 的 子 元 素 , 只 是 这 个 子 元 素 被 明确 标明 为 一 个 完整 的 模型 。 
(3) 场景 可 以 包含 任何 数量 的 模型 。 
场景 和 模型 都 可 以 包含 一 个 或 多 个 以 下 元 素 : 
对 象 (Objects) 
集合 (Collections) 
碰撞 对 象 (Collision objects) 
距离 对 象 (Distance objects) 
道 运动 学 组 (Inverse kinematics groups? 
几何 约束 求解 器 对 象 (Geometric constraint solver objects) 
子 脚本 (Child scripts) 
自 定义 用 户 界 面 (Custom user interfaces) 
除了 上 述 元 素 ,场景 还 将 包含 环境 、 主 脚本 、 页 面 和 视图 等 元 素 。 


3.3.2 V-REP 的 场景 


V-REP 中 的 场景 处 理 包含 一 个 或 多 个 模型 ,还 包括 了 环境 . 主 脚本 、 页 面 和 视图 。 

通过 与 视图 相关 联 的 可 视 对 象 可 以 看 到 场景 或 场景 图 像 内 容 , 该 视图 本 身 包 含 在 页 面 
中 。 创 建新 场景 时 (可 以 通过 依次 单 击 菜单 栏 . 文 件 、 新 场景 打开 ) ,默认 场景 将 包含 以 下 
元 素 : 
。 多 个 相机 对 象 : 如 果 相 机 与 视图 相关 联 , 则 相机 允许 查看 场景 。 
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几 个 光 对 象 : 没有 光 , 场 景 将 几乎 不 可 见 。 灯 光 用 于 照 亮 场景 。 
几 个 视图 : 视图 与 相机 相关 联 , 并 显示 相机 看 到 的 内 容 。 视 图 包含 在 页 面 中 。 
几 个 页 面 : 一 个 页 面包 含 一 个 或 多 个 视图 。 
环境 : 环境 由 环境 光 、 雾 .背景 颜色 等 属性 组 成 。 
地 板 : 地 板 由 组 合 在 一 个 模型 中 的 对 象 组 成 。 
默认 主 脚本 : 默认 主 脚本 应 该 允许 运行 最 少 的 仿真 ,而 不 需要 子 脚本 。 然 后 ,如 果 
与 场景 对 象 相关 联 , 则 稍 后 阶段 复制 到 场景 中 的 子 脚本 也 将 被 自动 执行 (由 主 脚 本 
调用 ) 。 

场景 可 以 通过 依次 单 击 菜单 栏 ,文件 ,打开 场景 … 打 开 ( 加 载 ), 并 可 以 通过 依次 单 击 菜 
单 栏 文件、 保存 场景 或 可 以 通过 依次 单 击 菜单 栏 .文件 ,将 场景 另存 为 … 保 存 场景 。 场 景 文 
TEC » .ttt”-files) 还 支持 在 资源 管理 器 窗口 和 应 用 程序 窗口 之 间 进 行 拖 放 操作 。 场 景 文件 
也 可 以 双击 打开 ,双击 后 系统 将 自动 启动 V-REP 应 用 程序 并 打开 场景 文件 。 在 打开 的 场 
景 之 间 切 换 可 以 通过 在 场景 层级 的 上 部 中 单 击 来 实现 (所 有 打开 的 场景 被 分 组 在 场景 层级 
的 顶部 ), 或 者 通过 其 相关 的 工具 栏 按钮 ( 见 图 3-18) 使 用 场景 选择 器 来 实现 。 


图 3-18 场景 选择 器 工具 栏 按钮 


环境 定义 了 场景 的 属性 和 参数 ,包括 了 背景 颜色 、 雾 参数 (与 相机 视觉 .传感器 相关 )、 环 
境 的 光线 ,场景 创建 的 信息 和 附加 的 设置 。 环 境 的 设置 对 话 框 可 以 通过 依次 单 击 Menu bar 
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(菜单 栏 )、Tools( 工 具 ) ,Environment( 环 境 ) 打 开 或 通过 双击 场景 层次 结构 中 对 应 的 场景 图 
标 打开 。 环 境 设置 对 话 框 见 图 3-19. 


Colors / Fog 


Backaround (up) | Ambient light 
Background (down) | Adjust fog parameters 
Wireless transmission visualization 
Visualize wireless emissions 


Visualize wireless reception: 


Calculation preprocessing 
Maximus triangle size (absolute) 00e-01 
Minimum triangle size (relative) 0. 020 
Save operation also saves existing calculation str 


Various 
Shape textures disabled 
Open&l-based custom user interface textures d 
Lock scene after next scene save 
Custom collision/contact respo Edit 
General callback script 


Extension strimpovrar [fosDist (4 00) f 


Clean-up object names Clean-up ghosts 


Scene content acknowledgments / Info 


图 3-19. 环境 设置 对 话 框 


默认 的 情况 下 ,每 个 场景 都 有 一 个 主 脚本 , 它 包 含 了 最 基本 的 代码 ,使 得 仿真 得 以 进行 。 
每 次 仿真 开始 和 结束 时 ,都 分 别 调用 一 次 主 脚本 。 因 此 , 主 脚 本 分 为 三 个 部 分 : 初始 化 部 
分 .常规 部 分 和 恢复 部 分 。 

CD 初始 化 部 分 负责 准备 用 于 仿真 的 代码 。 

(2) 常规 部 分 负责 处 理 所 有 的 代码 仿真 器 的 功能 ( 逆 运 动 学 .碰撞 检测 和 动力 学 等 ) ,其 
中 命令 simLaunchThreadedChildScripts 调用 线程 的 子 脚 本 。simHandleChildScripts 调用 
非 线程 的 子 脚本 。 这 一 部 分 分 为 驱动 (或 动作 /反应 ) 部 分 和 传 感 (或 探测 ) 部 分 。 

(3) 恢复 部 分 将 在 每 一 次 仿真 结束 时 执行 :这 部 分 的 代码 负责 恢复 对 象 的 初始 配置 、 清 
除 传感器 状态 .碰撞 状态 等 。 
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3.3.3 V-REP 的 模型 


1. 模型 的 创建 

模型 是 场景 的 子 元 素 。 模 型 本 身 不 能 被 仿真 , 它 必 须 被 包含 在 场景 中 ,然后 组 成 场景 以 
进行 操作 。 

模型 由 建立 在 同一 层次 树 上 的 场景 对 象 的 选择 来 定义 ,其 中 树 的 基础 必须 是 被 标记 为 
模型 基础 的 对 象 。 它 们 可 以 通过 依次 单 击 菜单 栏 .文件 ,加载 模 型 … 加 载 。 然 而 ,通过 在 模 
型 浏览 器 和 场景 视图 之 间 进 行 拖 放 操作 来 加 载 模型 更 容易 和 方便 。 模 型 可 以 通过 依次 单 击 
菜单 栏 文件 ,保存 模型 为 … 保 存 ,只 需 确 保 选 中 标记 为 对 象 的 单个 对 象 为 模型 库 的 基础 , 否 
则 "将 模型 男 存 为 …”" 菜 单项 将 不 被 启用 。 还 要 严格 遵循 关于 如 何 构 建 一 个 干净 的 仿真 模型 
的 教程 。 

模型 按 以 下 步骤 定义 : 

CD 将 多 辑 上 属于 该 模型 的 所 有 对 象 附加 到 基础 对 象 , 以 便 基础 对 象 是 模型 树 的 基础 。 

@ 检查 对 象 模型 基础 项 (objectismodelbase-item) 中 对 象 的 公共 属性 。 

© 在 上 面 的 同一 个 对 话 框 中 ,检查 对 象 /模型 是 否 可 以 转移 或 接受 DNA 项 目 。 如 果 稍 
后 需要 重建 修改 模型 ,这 将 简化 重建 的 过 程 。 

@ 在 上 面 的 同一 对 话 框 中 , 单 击 编辑 模型 属性 ,可 以 定义 特殊 覆盖 属性 (例如 ,使 整个 
模型 不 可 见 、 不 可 复制 等 )。 这 人 允许 快速 禁用 模型 中 定义 的 所 有 对 象 的 某 些 属性 。 

© 对 于 模型 中 的 所 有 对 象 ,除了 基础 对 象 之 外 ,请 检查 对 象 公 共 属 性 中 模型 的 选择 基 
础 。 这 将 保护 建 好 的 模型 : 用户 将 无 法 直接 选择 模型 中 的 单个 部 件 ,而 只 能 够 对 整个 模型 
进行 操作 。 

© 对 于 一 般 不 可 见 的 所 有 对 象 ,请 选中 不 显示 为 内 部 模型 选择 项 。 这 将 使 模型 边界 框 
在 模型 周围 以 正确 的 尺寸 显示 。 

CD 模型 的 作用 : 例如 你 的 模型 是 一 个 夹子 ,你 可 以 附加 到 操纵 器 的 手腕 (例如 ,用 户 建 
立 的 机 械 辟 模 型 可 以 接受 夹具 模型 的 操纵 器 )。 一旦 角色 决定 了 ,可 以 定义 模型 的 组 装 
行为 。 

现在 ,在 模型 基础 上 构建 的 单个 对 象 不 能 在 场景 中 被 选择 (选择 它们 将 改 为 选择 模型 的 
基础 ) ,但 是 仍然 可 以 通过 在 选择 期 间 按 住 Ctrl 和 Shift 键 在 场景 层次 中 选择 它们 。 除 此 之 
外 ,当选 择 基 础 对 象 时 ,包含 整个 模型 的 点 画 框 将 如 图 3-20 所 示 显 示 出 来 。 在 图 场景 视图 
和 相应 的 场景 层次 视图 中 , 选 定 的 虚拟 对 象 被 标记 为 模型 基础 对 象 。 

双击 模型 标签 打开 模型 对 话 框 , 其 中 可 以 调整 模型 属性 。 还 有 一 个 更 好 的 做 法 是 ,编辑 
模型 后 折 生 模型 的 层次 结构 ,如 图 3-21 所 示 , 以 便 轻松 识别 逻辑 上 分 组 的 元 素 / 模 型 的 
数量 。 

当 子 脚本 以 编程 方式 访问 对 象 时 ,将 多 个 对 象 分 组 为 模型 也 很 重要 。 请 记 住 ,在 V-REP 
中 ,对 象 /模型 可 以 在 仿真 期 间 随 时 重复 。 为 了 使 重复 的 子 脚本 能 够 访问 正确 的 对 象 (不 是 
原始 对 象 而 是 重复 的 对 象 ), 子 脚本 应 该 始终 与 它 访问 的 对 象 同时 复制 。 一 种 保证 方式 是 创 
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图 3-20 ”基础 对 象 与 层次 结构 


建 模型 (如 上 所 述 ) ,并 确保 访问 模型 中 对 象 的 子 脚本 与 模 
型 中 包含 的 对 象 相关 联 。 最 好 是 将 一 个 子 脚本 (也 可 能 有 [8 mme ome a 
辅助 子 脚 本 ) 与 模型 的 基础 相关 联 。 ; 

为 了 使 模型 能 够 容易 地 组 合 ( 即 在 彼此 之 上 构建 ), 而 
没有 任何 额外 的 修改 ,重要 的 是 考虑 模型 将 扮演 什么 角色 : 
它 会 附加 到 其 他 型 号 ,还 是 会 接受 其 他 型 号 附加 给 它 ? 这 
些 问题 的 答案 将 允许 选择 最 佳 对 象 类 型 作为 模型 基础 。 

复制 和 粘贴 模型 的 行为 与 先 保存 模型 ,然后 加 载 它 (使 
用 内 存 缓冲 区 而 不 是 磁盘 空间 ) 的 操作 完全 相同 。 模 型 可 图 3-21 显示 SAAR 
以 像 任何 其 他 对 象 一 样 从 一 个 场景 复制 到 另 一 个 场景 。 模 场景 层次 视图 
型 文件 (** .ttmy-files) 还 支持 在 资源 管理 器 窗口 和 应 用 
程序 窗口 之 间 进 行 拖 放 操作 。 也 可 以 双击 模型 文件 ,随后 系统 将 启动 V-REP. 应 用 程序 ,并 
加 载 到 默认 场景 中 。 

2. 模型 对 话 框 的 设置 

模型 的 属性 可 以 在 模型 对 话 框 中 单独 调整 。 可 以 通过 双击 场景 层次 结构 中 的 模型 图 标 
来 打开 它 ,如 图 3-22 Be. 

选择 模型 缩 略 图 : 保存 模型 时 ,会 弹出 一 个 对 话 框 .要求 输入 模型 缩 略 图 (将 显示 在 模 
型 浏览 器 中 )。 但 是 ,如 果 希 望 以 不 同 的 配置 保存 模型 的 缩 略 图 (例如 ,希望 以 直线 配置 保存 
蛇 形 机 器 人 的 模型 ,但 希望 缩 略 图 以 弯曲 配置 可 视 化 蛇 形 机 器 人 ), 则 可 以 在 此 处 指定 缩 
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Model Properties 


Select model thumbnail 
Override properties 

Model is not visible .. Model is not colidable 
Model s not measurable. Model is not renderable. 
Model i not cuttable Model is not detectable. 
Model is not dynamic Model is not respondabie 
Model scripts are inactive (chid scripts and customization scripts) 
Invisible to other modef s bounding boxes 


Model content acknowledgments/info. 
The model is courtesy of Annick, Yann, Yves, Nora, Niels and Maxim. 


图 3-22 模型 状态 对 话 框 


咯 图 。 

覆盖 属性 : 在 这 里 ,可 以 禁用 (覆盖 ) 整 个 模型 ( 即 模型 层次 结构 树 中 的 所 有 对 象 ) 的 特 
定 属性 。 这 样 便于 快速 禁用 需要 太 多 计算 时 间 的 模型 。 

模型 内 容 确认 /信息 : 与 模型 相关 的 信息 。 始 终 良好 的 做 法 是 确认 场景 ,模型 或 导入 网 
格 的 原始 作者 。 当 包含 确认 信息 的 模型 打开 时 , 它 将 自动 显示 该 信息 。 


3.3.4 V-REP 的 环境 


V-REP 中 的 环境 定义 作为 场景 一 部 分 的 属性 和 参数 ,但 不 是 场景 对 象 。 保 存 模型 时 ， 
不 保存 环境 属性 和 参数 ,环境 的 属性 和 参数 仅 在 保存 场景 时 保存 。 

环境 定义 以 下 属性 和 参数 : 

。 背景 颜色 。 

。 雾 参数 。 雾 参数 不 直接 与 场景 对 象 交互 ,除非 使 用 相机 或 视觉 传感器 环境 光 。 

。 场景 创建 信息 。 

。 其 他 设置 。 

1. 环境 设置 对 话 框 

可 以 通过 依次 单 击 菜单 栏 .工具 、 环 境 或 通过 双击 场景 层次 结构 中 的 图 标 访问 环境 对 话 
框 ( 见 图 3-23) 。 

如 图 3-23 所 示 ,对 环境 设置 对 话 框 的 说 明 如 下 : 

COD 背景 (向 上 /向 下 ): 人 允许 调整 场景 的 背景 颜色 。 向 上 分 量 对 应 于 屏幕 的 上 部 (天 
空 ) ,向 下 分 量 对 应 于 屏幕 的 下 部 分 。 背 景 颜色 仅 在 禁用 雾 功能 时 可 见 。 
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[300e01 
[0.020 _ 


Extension sting |povray (fogosst (4.00) fogTransp {0.50)} 
Geanup objectnames | Gean-up ghosts 


Scene content adnomedgments / Info 


Scene aeated by Marc 
The mesh models pr er eh 
The controllers are courtesy of Nora, 


图 3-23 ”环境 设置 对 话 框 


(2) 环境 光 : 允许 调整 场景 的 环境 光 。 环 境 光 可 以 被 视 为 场景 的 最 小 光 , 它 以 完全 相 
同 的 方式 从 所 有 方向 照 亮 对 象 。 

(3) 调整 雾 参 数 : 允许 调整 各 种 雾 参 数 。 

(4) 可 视 化 无 线 发 射 /接收 : 如 果 启 用 , 则 所 有 无 线 发 射 /接收 活动 将 被 可 视 化 。 

(5) 最 大 三 角形 大 小 (绝对 ): 此 项 不 会 影响 形状 的 视觉 外 观 。 然 而 ,这 将 影响 大 多 数 
V-REP 计算 模块 的 执行 速度 。 例 如 ,在 两 个 实体 之 间 执 行 最 小 距离 计算 时 ,如 果 两 个 实体 
由 大 小 基本 相同 的 三 角形 组 成 , 则 执行 通常 更 快 。 最 大 三 角形 尺寸 值 指 定 如 何 处 理 形 状 的 
内 部 表示 ( 即 形状 的 计算 结构 是 多 么 精细 )。 小 尺寸 会 增加 预 处 理 时 间 ,但 一 般 来 说 ,仿真 执 
行 速 度 会 同时 提高 。 此 值 将 整个 最 大 三 角形 的 大 小 设置 为 绝对 值 。 

C6) 最 小 三 角形 大 小 (相对 ): 类 似 于 上 一 项 ,但 此 项 有 助 于 避免 创建 可 能 需要 很 长 时 
间 的 过 大 的 计算 结构 。 此 值 将 最 小 三 角形 大 小 设置 为 相对 值 ( 相 对 于 给 定 对 象 的 最 大 维 
度 ) 。 保 存 操作 还 保存 现 有 的 计算 结构 : 对 于 距离 计算 碰撞 检测 等 ,在 仿真 开始 时 ( 预 处 理 ) 
计算 数据 结构 ,或 者 在 这 种 计算 中 第 一 次 涉及 形状 ,以 便 加 速 计算 。 该 数据 结构 的 计算 可 能 
是 耗 时 的 ,因此 用 户 可 以 选择 将 其 与 场景 或 模型 一 起 保存 。 然 而 ,必须 意识 到 ,将 被 保存 的 
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附加 信息 是 大 的 ,这 将 导致 总 的 文件 变 得 很 大 (有 时 是 两 倍 或 更 大 ) 。 

(7) 禁用 形状 纹理 : 如 果 选 中 , 则 将 禁用 应 用 于 形状 的 所 有 纹理 。 

(8) 自 定义 用 户 界 面 纹理 已 禁用 : 如 果 选 中 , 则 将 禁用 应 用 于 基于 OpenGl 的 自 定义 
UI 的 所 有 纹理 。 

(9) 在 下 一 个 场景 保存 后 锁定 场景 : 如 果 要 在 编辑 /修改 .查看 脚本 内 容 . 导 出 资源 这 
些 操作 中 锁定 场景 ,可 以 选择 该 选项 。 如 果 和 希望 以 后 能 够 修改 它 , 请 确保 已 经 在 解锁 状态 下 
保存 了 相同 的 场景 。 

(10) 自 定义 碰撞 /联系 人 响应 : 选中 此 项 目 后 ,所 有 动态 联系 人 处 理 都 可 以 由 用 户 定 
义 ,并 通过 联系 人 回调 脚本 执行 。 请 记 住 ,此 设置 与 场景 相关 ,模型 将 在 不 同 的 场景 中 作出 
不 同 的 表现 。 此 设置 仅 适用 于 知道 它们 正在 做 什么 的 用 户 。 

(11) 一 般 回调 脚本 : 当选 中 此 项 时 ,将 能 够 处 理 普通 回调 脚本 中 的 特定 用 户 回调 。 当 
插件 调用 API 函数 simHandleGeneralCallbackScript 时 ,将 调用 通用 回调 脚本 。 

(120 扩展 字符 串 : 描述 其 他 环境 属性 的 字符 串 , 主要 由 扩展 插件 使 用 (与 
simGetExtensionString API 函数 有 关 ) 。 

(13) 清理 对 象 名称 : 允许 使 用 哈 希 标记 将 某 些 顺序 放 和 人 对 象 名 称 中 。 这 不 是 必需 的 ， 
但 可 以 方便 地 减少 哈 希 标记 后 的 后 绷 数 。 

(14) 清除 重 影 : 删除 场景 可 能 包含 的 所 有 重 影 对 象 ,这 与 重 影 记 录 功 能 有 关 。 

(15) 场景 内 容 确认 /信息 : 与 场景 有 关 的 信息 。 始 终 良 好 的 做 法 是 确认 场景 .模型 或 
导入 网 格 的 原始 作者 。 当 打开 包含 确认 信息 的 场景 时 , 它 将 自动 显示 该 信息 。 

2. 纹理 设置 对 话 框 

纹理 对 话 框 如 图 3-24 所 示 , 它 的 作用 是 允许 查看 和 修改 与 附加 到 形状 或 基于 OpenGI 
的 自 定 义 UI 的 纹理 相关 的 属性 。 


Texture Properties 
Remove texture Load new texture 
Texture relative positionjorientation. Texture scaling 


Other texture properties 

Mapping mode ookction — 7 

Apph mode [mddate ~] [Repeat alongu 
[7] interpolate colors C Repest along v 
Texture info 


图 3-24 纹理 设置 对 话 框 
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可 通过 在 形状 属性 对 话 框 中 单 击 设置 纹理 项 ,或 通过 单 击 基 于 OpenG1 的 自 定义 UI 对 
话 框 中 的 设置 UI 背景 纹理 项 或 设置 按钮 纹理 项 来 访问 它 。 

纹理 是 位 图 图 像 , 可 以 应 用 到 表面 ,以 使 它们 看 起 来 更 真实 。 想 象 一 下 ,将 砖 纹理 应 用 
到 和 矩形 表面 ,以 使 其 看 起 来 像 砖 墙 。V-REP 对 形状 应 用 纹理 的 默认 方式 是 将 其 投影 到 形状 
的 X/Y FHE. 

V-REP 支持 以 下 5 种 纹理 映射 方法 : 

(1) 投影 映射 : 将 纹理 简单 地 投影 到 对 象 的 X-Y 平面 上 。 将 计算 纹理 坐标 。 

(2) 圆柱 映射 : 纹理 包 里 在 对 象 的 Z 轴 周围 。 将 计算 纹理 坐标 。 

(3) 球面 映射 : 纹理 被 球面 映射 到 对 象 上 。 将 计算 纹理 坐标 。 

(D 框 映射 : 纹理 应 用 于 盒 状 对 象 的 所 有 6 个 面 。 将 计算 纹理 坐标 。 

(5) 导入 的 纹理 坐标 : 通过 OBJ 文件 格式 ,可 以 在 导入 网 格 的 同时 导入 特定 的 纹理 
坐标 。 

建议 保持 默认 设置 ,以 保持 与 较 旧 的 显卡 的 兼容 性 (当选 中 缩放 纹理 时 ,所 有 纹理 都 将 
调整 为 2 的 寡 次 ) 和 文件 大 小 。 

一 旦 纹理 在 存储 器 中 , 它 可 以 在 实际 映射 发 生 之 前 被 缩放 \ 移 位 和 旋转 ,以 便 获得 期 望 
的 视觉 外 观 。 

如 果 将 纹理 应 用 于 作为 背景 的 基于 OpenGI 的 自 定义 UI 或 基于 OpenGI 的 自 定义 UI 
按钮 , 则 它 不 能 被 缩放 ,移动 或 旋转 。 如 图 3-25 和 图 3-26 所 示 , 它 将 自动 地 在 整个 自 定义 
UI 表面 或 整个 自 定义 UI 创建 按钮 。 


图 3-25 应 用 于 基于 OpenG1 的 自 定义 图 3-26 ”应 用 于 基于 OpenG1 的 自 定义 
UI 作为 背景 的 纹理 UI 作为 按钮 的 纹理 


基于 OpenGl 的 自 定义 UI 背景 纹理 仅 在 按钮 具有 选择 透明 /显示 背景 纹理 项 目 时 才 可 
见 。 通 过 单 击 加 载 新 纹理 按钮 可 以 加 载 静态 纹理 。 目 前 支持 以 下 文件 格式 : JPEG、PNG、 
TGA,BMP,TIFF fil GIF, 

除了 从 文件 加 载 纹理 ,还 可 以 选择 已 加 载 的 静态 纹理 ,或 由 视觉 传感器 生成 的 动态 纹 
理 。 只 需 单 击 “ 从 现 有 纹理 选择 纹理 按钮 " 即 可 替换 纹理 。 注 意 ,动态 纹理 只 有 在 仿真 期 间 ， 
而 且 视觉 传感器 被 正确 处 理 的 时 候 , 才 能 够 正常 显示 出 来 。 
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3.4 实体 


模型 包括 了 以 下 这 些 元 素 : 对 象 . 集 合 、 磁 撞 的 对 象 . 远 程 对 象 逆 运动 学 组 .几何 约束 
求解 对 象 . 子 脚本 和 自 定义 用 户 界面 。 其 中 一 个 场景 对 象 或 集合 称 为 实体 。 场 景 、 场 景 对 
象 .集合 .实体 的 关系 如 图 3-27 所 示 , 对 象 A .对象 B、 对 象 C、 对 象 D, 集 合 1 和 集合 2 都 称 
为 实体 。 


图 3-27 场景 场景 对 象 集 合 和 实体 的 关系 


3.4.1 V-REP 的 场景 对 象 


一 个 V-REP 仿真 场景 包含 了 一 些 场景 对 象 和 基本 对 象 。 这 些 对 象 由 树 状 的 层次 结构 
装配 在 一 起 ,共同 组 成 了 V-REP 中 的 仿真 模型 和 工作 环境 。 在 V-REP 界面 中 ,对 象 存在 
于 场景 层次 结构 和 可 见 的 场景 视图 中 。 

如 图 3-28 所 示 , V-REP 中 所 支持 的 场景 对 象 包括 : 形状 (shape) .关节 (joint) 路径 
(path)、 摄 像 机 (camera)、 光 源 (light)、 近 距离 传感器 (proximitysensor)、 视 觉 传感器 
(visionsensor) 、 力 /力矩 传感器 (force/torquesensor)、 图 表 (graph)、 麻 机 (mill)、 镜 子 
(mirror) 等 。 这 些 场 景 对 象 部 分 由 V-REP 本 身 提供 ,部 分 可 从 外 部 的 建 模 软件 导入 。 下 面 
对 模型 的 建立 所 涉及 的 场景 对 象 进行 介绍 。 


形状 aen atesa rr ë 
0606 gere ? E i m k 
E xm dX Ji nent me \ 
222 | aoo å $ E v2 
Ages LE 1 仿制 品 < en Á 了 nr 
E < e e E 489 
图 3-28 V-REP 中 的 场景 对 象 示意 图 


形状 : 它 是 由 三 角形 网 络 构成 的 刚性 网 络 ,一 方面 用 于 刚体 的 仿真 , 另 一 方面 实现 了 刚 
体 的 可 视 化 。 形 状 是 可 碰撞 、 可 衡量 .可 检测 、 可 泻 染 和 可 放 缩 的 对 象 , 所 以 它 可 用 于 与 其 他 
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对 象 的 碰撞 检测 、 可 被 近 距 离 传感器 、 视 觉 传 感 器 等 检测 。 因 此 ,其 他 场景 对 象 的 使 用 也 在 
很 大 程度 上 依赖 于 形状 。 

关节 : 它 是 一 种 连接 两 个 或 多 个 场景 对 象 的 元 素 , 并 赋予 它们 1 个 或 3 个 自由 度 。 关 
节 具有 4 种 类 型 : 转动 关节 、 移 动 关节 、 螺 丝 关 节 和 球形 关节 ,前 3 种 具有 1 个 自由 度 ,球形 
关节 具有 3 个 自由 度 。 

路 径 : 它 允 许 对 空间 中 复杂 的 运动 进行 定义 ,包括 转化 、 旋 转 或 暂停 以 及 它们 之 间 的 组 
合 , 并 使 机 器 人 沿 着 一 个 预定 义 的 轨迹 进行 运动 。 例 如 ,让 焊接 机 器 人 沿 着 预定 轨迹 进行 焊 
接 物 体 表面 的 工作 ,或 者 让 传送 带 沿 预定 轨迹 运动 。 

摄像 机 : 摄像 机 允许 从 不 同 的 角度 去 观察 仿真 的 场景 。 

磨 机 : 可 以 在 形状 对 象 上 执行 切割 操作 。 

力 /力矩 传感器 : 能 够 测量 电机 的 力 和 力矩 。 

视觉 传感器 : 能 够 与 空间 中 的 光 ,颜色 和 图 片 反应 。 

近 上 距离 传感器 : 能 够 在 它 的 检测 范围 内 精确 地 检测 一 个 物体 。 

图 表 : 图 表 可 以 用 来 记录 和 可 视 化 仿真 数据 。 

Bi. 能 够 反射 图 像 ,也 可 以 作为 辅助 剪 切 面 操作 。 

光源 : 能 够 照明 仿真 的 场景 。 

下 面 详细 地 介绍 V-REP 中 的 各 种 场景 对 象 。 


3.4.2 场景 对 象 的 性 质 


一 些 上 述 对 象 可 以 具有 人 允许 其 他 对 象 或 计算 模块 与 它们 交互 的 特殊 属性 。 对 象 可 
以 是 : 

(1) 可 碰撞 : 碰撞 的 对 象 可 以 与 其 他 可 碰撞 的 对 象 发 生 碰撞 。 

(2) 可 测量 的 : 可 测量 的 对 象 可 以 具有 它们 和 其 他 可 测量 的 对 象 之 间 的 最 小 距离 。 

(3) 可 检测 的 : 可 由 接近 传感器 检测 可 检测 的 物体 。 

(4) 可 切割 : 可 切割 的 物体 可 以 被 铣 刀 切 割 。 

(5) 可 泻 染 : 可 泻 染 的 对 象 可 以 被 视觉 传感器 看 到 或 检测 。 

(6) 可 查看 : 可 查看 对 象 可 以 通过 “查看 ”选项 查看 ,它们 的 图 像 内 容 也 可 以 在 视图 中 
可 视 化 。 

每 个 对 象 在 仿真 场景 内 具有 位 置 和 取向 。 对 象 的 位 置 和 方向 称 为 对 象 的 配置 。 对 象 可 
以 附加 到 其 他 对 象 (或 构建 在 彼此 之 上 )。 如 果 对 象 A 构建 在 对 象 B 的 顶部 , 则 对 象 B 是 父 
对 象 , 对 象 A 是 子 对 象 。 要 在 对 象 B 和 对 象 A 之 间 创建 父子 关系 ,请 选择 对 象 A, 然 后 选择 
对 象 B( 选 择 顺序 很 重要 )。 然 后 依次 选择 菜单 栏 编 辑 ` 最 后 选择 的 对 象 父 。 如 图 3-29 所 
示 , 它 说 明了 此 操作 对 模型 关系 的 影响 。 

也 可 以 将 对 象 拖 放 到 场景 层次 结构 中 的 另 一 个 上 ,以 获得 类 似 的 结果 。 注 意 : 对 象 A 
的 配置 没有 改变 (两 个 对 象 保持 它们 各 自 的 配置 )。 然 而 ,查看 场景 层次 结构 ,可 以 看 到 对 象 
人 成 为 对 象 B 的 子 对 象 。 如 果 现 在 移动 对 象 B, 则 对 象 A 将 自动 跟随 ,因为 对 象 A 附加 到 
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(a) 在 将 对 象 A 附 加 到 对 象 B 之 前 (b) 在 将 对 象 A 附 加 到 对 象 B 之 后 


图 3-29 场景 对 象 对 模型 关系 的 影响 


对 象 B。 对 象 A 可 以 通过 选择 它 来 分 离 , 然 后 依次 选择 菜单 栏 .编辑 .选择 孤立 的 对 象 。 这 
样 做 将 分 离 对 象 A, 而 不 更 改 其 配置 。 也 可 以 将 对 象 拖 放 到 世界 图 标 上 ,以 获得 类 似 的 
结果 。 

每 个 对 象 都 具有 相对 于 世界 参考 坐标 系 的 绝对 配置 (或 累积 配置 ) ,以 及 相对 于 父 对 象 
参考 坐标 系 的 本 地 配置 (或 相对 配置 )。 在 上 面 的 例子 中 , 当 对 象 A 成 为 对 象 B 的 子 对 象 
时 ,对 象 A 的 绝对 配置 没有 改变 ,但 它 的 本 地 配置 被 修改 。 

最 后 选择 的 对 象 的 绝对 配置 显示 在 信息 文本 中 , 某 些 对 象 数据 可 以 通过 图 形 对 象 进行 
iix. 

1. 场景 对 象 的 具体 形状 

1) 可 碰撞 的 对 象 

可 碰撞 的 对 象 是 可 以 被 测试 与 其 他 可 碰撞 对 象 碰撞 的 对 象 ,即将 注册 碰撞 状态 。 这 并 
不 意味 着 它们 将 响应 冲突 ( 即 可 响应 ) ,这 是 不 同 的 东西 。 可 碰撞 的 对 象 包括 : 虚拟 对 象 , 形 
状 , 八 又 树 ,点 云 。 

可 碰撞 对 象 可 以 单独 启用 或 禁用 其 可 碰撞 的 属性 (默认 情况 下 为 非 纯 形状 、 八 叉 树 和 点 
云 启 用 )。 这 可 以 在 对 象 公共 属性 中 设置 或 通过 simSetObjectSpecialProperty API 函数 设 
置 。 此 外 ,可 碰撞 的 对 象 可 以 根据 其 相关 的 模型 属性 (如 果 它 们 是 模型 的 一 部 分 ) 覆 盖 其 可 
碰撞 的 属性 。 

2) 可 测量 的 对 象 

可 测量 的 对 象 是 可 用 于 与 其 他 可 测量 的 对 象 进行 最 小 距离 计算 的 对 象 ,包括 : 虚拟 对 
象 ,形状 , 八 又 树 ,点 云 。 集 合 也 是 可 衡量 的 ,因为 它们 可 能 包含 可 衡量 的 对 象 。 

可 测量 的 对 象 可 以 单独 启用 或 禁用 其 可 测量 的 属性 (对 于 非 纯 形 状 , 八 又 树 和 点 云 默 认 
启用 )。 这 可 以 在 对 象 公 共 属 性 中 设置 或 通过 simSetObjectSpecialProperty API 函数 设置 。 

此 外 ,可 衡量 对 象 可 以 根据 其 相关 的 模型 属性 (如 果 它 们 是 模型 的 一 部 分 ) 覆 盖 其 可 衡 
量 属性 。 
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3) 可 检测 的 对 象 

可 检测 的 对 象 是 可 由 接近 传感器 检测 的 对 象 , 包 括 : 虚拟 对 象 ,形状 , 八 又 树 , 点 云 虚 拟 
对 象 和 点 云 ,由 于 基于 点 ,它们 不 能 被 射线 型 或 随机 型 接近 传感器 检测 到 。 

可 检测 的 对 象 可 以 由 所 有 接近 传感器 或 仅 由 特定 类 型 的 接近 传感器 或 如 下 所 列 的 接近 
传感器 的 子 类 别 检测 : 超声 波 接近 传感器 ,红外 接近 传感器 ,激光 接近 传感器 ,电感 式 接近 
传感器 ,电容 式 接 近 传 感 器 。 

集合 也 是 可 检测 的 ,因为 它们 可 能 包含 可 检测 的 对 象 。 可 检测 对 象 的 可 检测 的 属性 可 
以 单独 启用 或 禁用 ,这 适用 于 所 有 类 型 的 接近 传感器 (默认 情况 下 ,对 于 非 纯 形 状 启用 )。 这 
可 以 在 对 象 公共 属性 中 设置 或 通过 simSetObjectSpecialProperty API 函数 设置 。 

此 外 ,可 检测 的 对 象 可 以 根据 其 相关 的 模型 属性 (如 果 它 们 是 模型 的 一 部 分 ) 覆 盖 其 可 
检测 的 属性 。 

4) 可 切割 的 对 象 

可 切割 物体 是 可 以 被 铣 刀 切割 的 物体 。 现 在 ,只 有 简单 的 非 纯 形 状 是 可 以 切割 的 ,但 未 
来 的 V-REP 版 本 也 将 包括 可 切割 的 卷 。 集 合 也 是 可 切割 的 ,因为 它们 可 能 包含 可 切割 
对 象 。 

可 切割 的 对 象 可 以 单独 启用 或 禁用 它们 的 可 切割 属性 (默认 情况 下 禁用 )。 这 可 以 在 对 
象 公共 属性 中 设置 或 通过 simSetObjectSpecialProperty API 函数 设置 。 

此 外 ,可 切割 对 象 可 以 根据 它们 的 相关 模型 属性 (如 果 它 们 是 模型 的 一 部 分 ) 对 其 可 切 
割 属 性 进行 重 写 。 

5) 可 泻 染 对 象 

可 泻 染 对 象 是 可 由 视觉 传感器 看 到 或 检测 到 的 对 象 ,包括 : 形状 ,路 径 ( 路 径 整 形 功能 
必须 启用 ) ,图 形 ( 仅 泻 染 非 静态 3D 曲线 ) ,镜子 , 八 又 树 ,点 云 。 

集合 也 是 可 呈现 的 ,因为 它们 可 能 包含 可 呈现 对 象 。 

你 可 以 有 一 个 可 泻 染 的 对 象 只 有 特定 的 视觉 传感器 才能 看 到 。 可 渲染 对 象 也 可 以 单独 
启用 或 禁用 它们 的 可 呈现 属性 (默认 情况 下 启用 ,纯粹 形状 除外 )。 这 可 以 在 对 象 公共 属性 
中 设置 或 通过 simSetObjectSpecialProperty API 函数 设置 。 

此 外 ,可 泻 染 对 象 可 以 根据 其 相关 的 模型 属性 (如 果 它 们 是 模型 的 一 部 分 ) 覆 盖 其 可 演 
染 属 性 。 

6) 可 见 对 象 

可 见 对 象 是 可 以 浏览 .查看 或 可 以 显示 某 些 图 像 内 容 的 对 象 , 包 括 : 相机 ,图 表 , 视 觉 传 
感 器 ,可 见 对 象 可 以 与 将 显示 其 图 像 内容 的 视图 相关 联 。 

2. 场景 对 象 的 性 质 设 置 

依次 单 击 菜单 栏 . 工 具 、 场 景 对 象 属性 ,可 以 打开 场景 对 象 属性 对 话 框 。 还 可 以 通过 双 
击 场景 层次 结构 中 的 对 象 图 标 或 单 击 其 工具 栏 按钮 来 打开 对 话 框 。 

如 图 3-30 所 示 ,场景 对 象 属性 对 话 框 显 示 与 对 象 ( 即 场景 对 象 ) 相 关 的 属性 。 对 话 框 是 
上 下 文敏 感 的 ,其 内 容 主要 取决 于 场景 对 象 选 择 状 态 : 只 显示 最 后 选择 的 对 象 的 属性 。 这 


第 3 章 ”V-REP 在 机 器 人 仿真 中 的 应 用 P 231 


些 属性 分 为 两 部 分 。 
Shape Common. 
General properties 
v Selectable Irvasble aurrg selechon 
Select base of model instead Ignored by depth pass 
Don't show as inside model selection. 
Size factor. 1.000e400 Ignored for view-fitting 
Extension string 
Apply to selection. 


Can be seen by al cameras and vison sensors ~| 
Appi to selection 
Object speaal propertes 
Y Colidable V Measurable * Detectable | detais 
Y Renderable. Cuttable 
Apply to selection. 
Model definition 
Object is model base Edit model properbes. 
Other 
Object / model can transfer or accept DNA 
Collection self-colision indicator 0 
Saing J | Vew|edtastomdata | 
Assemblng | 


图 3-30 Scene 对 象 属性 对 话 框 ,当前 显示 对 象 的 常用 属性 


。 对 象 类 型 特定 属性 : 特定 于 所 选 对 象 类 型 的 属性 。 

。 对 象 公共 属性 : 所 有 对 象 类 型 共有 的 属性 。 

对 话 框 上 部 的 2 个 按钮 允许 选择 要 显示 的 属性 类 型 。 如 果 对 象 选择 为 空 , 则 所 有 对 话 
框 项 都 将 处 于 非 活 动 状态 。 

对 话 框 的 对 象 类 型 特定 属性 部 分 将 显示 以 下 对 话 框 之 一 ,具体 取决 于 最 后 选择 的 对 象 
的 类 型 。 但 是 ,场景 对 象 具 有 一 些 公 共 的 属性 ,能 够 在 对 话 框 中 被 设置 。 

在 场景 对 象 属性 对 话 框 中 , 单 击 常用 按钮 以 显示 对 象 常用 属性 对 话 框 。 该 对 话 框 显示 
上 次 选择 的 对 象 的 设置 和 参数 。 如 果 未 选择 对 象 , 则 对 话 框 处 于 非 活动 状态 。 如 果 选 择 了 
多 个 对 象 , 则 可 以 将 一 些 参 数 从 最 后 选择 的 对 象 复制 到 其 他 所 选 对 象 (应 用 于 选择 按钮 ) 。 

"E. 指示 是 否 可 以 在 场景 中 选择 对 象 。 总 是 可 以 在 场景 层次 结构 中 选择 对 象 。 与 
simSetObjectProperty 函数 有 关 。 

在 选择 期 间 不 可 见 : 启用 时 ,对 象 将 在 选择 过 程 中 不 可 见 ( 即 ,我 们 将 能 够 通过 对 象 进 
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行 选择 ) 。 

忽略 深度 传递 : 当 启 用 时 ,在 深度 泻 染 过 程 中 对 象 将 被 忽略 。 深 度 泻 染 通道 用 于 正确 
定位 红色 球体 以 用 于 相机 移动 。 

选择 模型 的 基础 : 如 果 启 用 ,那么 选择 场景 中 的 对 象 将 选择 其 第 一 个 父 对 象 标 记 为 对 
象 是 模型 基础 ( 详 见 下 文 )。 当 保护 模型 免 受 错误 操作 时 ,此 属性 很 方便 ,允许 将 其 作为 单个 
实体 与 其 他 对 象 一 起 操作 。 这 与 simSetObjectProperty 函数 有 关 。 

不 显示 为 内 部 模型 选择 : 当选 择 时 ,并 且 对 象 是 模型 的 一 部 分 , 则 模型 边界 框 ( 即 模型 
选择 边界 框 ) 将 不 会 包含 该 对 象 。 这 对 于 可 能 使 模型 边界 框 显 得 太 大 的 不 可 见 对 象 很 有 用 。 
此 属性 没有 功能 效果 。 它 与 simSetObjectProperty KAA X. 

尺寸 因子 : 每 个 对 象 可 以 在 任何 时 间 ,也 在 仿真 期 间 缩 放 (调整 大 小 )。 尺 寸 因子 将 以 
类 似 的 方式 缩放 ,并 且 可 以 以 编程 方式 访问 以 调整 代码 的 行为 (例如 子 脚本 )。 设 想 一 个 两 
轮 运动 机 器 人 ,其 运动 通过 子 脚本 以 简单 的 方式 控制 : 子 脚本 将 根据 几 个 参数 (车 轮 旋转 速 
度 .车 轮 直径 和 两 个 车 轮 之 间 的 距离 ) 计 算 机 器 人 的 新 位 置 。 如 果 用 户 缩放 机 器 人 , 子 脚本 
应 根据 新 的 尺寸 参数 (车 轮 直径 和 两 个 车 轮 之 间 的 距离 ) 调 整 其 计算 。 它 可 以 通过 使 用 
simGetObjectSizeFactor API 函数 来 实现 。 

忽略 视图 拟 合 : 在 没有 选择 对 象 的 情况 下 将 场景 添加 到 视图 时 ,将 不 会 考虑 使 用 所 选 
项 目的 对 象 。 通 常 ,楼层 将 被 这 样 标记 。 它 与 simCameraFitToView API 函数 有 关 。 

扩展 字符 串 : 描述 其 他 对 象 属性 的 字符 串 , 主 要 由 扩展 插件 使 用 ,与 simGetExtensionString 
API 函数 有 关 。 

相机 可 见 性 图 层 : V-REP 中 的 每 个 对 象 可 以 分 配给 一 个 或 多 个 可 见 性 图 层 。 如 果 存 
在 至 少 一 个 与 层 选择 对 话 层 匹 配 的 可 见 层 , 则 当 从 相机 看 时 ,该 对 象 将 是 可 见 的 。 默 认 情 况 
下 ,形状 分 配给 第 一 层 ,第 二 层 的 关节 、 第 三 层 的 虚拟 等 。 可 以 看 出 : 允许 指定 将 是 唯一 能 
够 看 到 对 象 的 相机 或 视觉 传感器 (或 包含 相机 或 视觉 传感器 的 集合 ) 。 

Collidable: 允许 启用 或 禁用 所 选 可 collidable 对 象 的 碰撞 检测 功能 。 

可 测量 : 允许 启用 或 禁用 所 选 可 测 对 象 的 最 小 距离 计算 能 力 。 

可 检测 : 允许 启用 或 禁用 所 选 可 检测 对 象 的 接近 传感器 检测 能 力 。 单 击 详细 信息 将 多 
许 编辑 可 检测 的 详细 信息 。 

可 泻 染 : 允许 启用 或 禁用 所 选 可 演 染 对 象 的 视觉 传感器 检测 能 力 。 

可 切割 : 允许 启用 或 禁用 所 选 可 切割 对 象 的 铣 刀 切割 能 力 。 

对 象 是 模型 基础 : 指示 对 象 是 否 应 作为 模型 的 基础 。 标 记 为 模型 基础 的 对 象 具有 特殊 
属性 (例如 ,保存 或 复制 对 象 也 将 自动 保存 /复制 其 所 有 子 项 和 子 项 的 子 项 等 )。 另 外 ,当选 
择 这 样 的 对 象 时 ,选择 边界 框 被 显示 为 粗 点 画 线 ,包围 整个 模型 。 参 考 模型 ,并 选择 上 面 的 
模型 的 选择 基础 。 

编辑 模型 属性 : 允许 打开 模型 对 话 框 。 

对 象 /模型 可 以 传输 或 接受 DNA: 当 为 对 象 或 模型 启用 此 功能 时 , 它 将 与 其 所 有 副本 
共享 相同 的 标识 符 。 对 象 或 模型 然后 可 以 经 由 转移 DNA 工具 条 按钮 将 其 DNA( 即 ,复制 
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其 自身 的 实例 ) 转 移 到 其 所 有 兄弟 ( 即 具有 相同 标识 符 的 对 象 /模型 )。 想 象 一 下 ,在 你 的 场 
景 中 有 100 个 相同 的 机 器 人 ,你 想 以 类 似 的 方式 修改 : 只 需 修改 其 中 之 一 ,选择 它 , 然 后 单 
击 转 移 DNA 工具 栏 按 钮 。 这 个 项 目 应 该 总 是 检查 一 个 模型 基础 ( 见 后 续 ) ,以 方便 模型 
重建 。 

集合 自 冲突 指示 器 : 当 在 两 个 相同 集合 之 间 执 行 冲突 (或 最 小 距离 ) 计 算 时 , V-REP 通 
常 将 检查 所 有 集合 项 目 与 该 集合 中 的 所 有 其 他 项 目 。 在 某 些 情况 下 ,例如 运动 链 , 人 们 不 想 
检查 连续 的 连 杆 ,因为 它们 可 能 在 接口 处 不 断 地 碰撞 。 在 这 种 情况 下 ,可 以 使 用 集合 自 冲突 
者 示 器 : 如 果 它 们 的 指标 差异 正好 为 1, 则 相同 集合 中 的 两 个 项 目 不 会 相互 检查 。 

缩放 : 对 象 或 模型 可 以 在 V-REP 中 以 灵活 的 方式 缩放 。 对 象 或 模型 的 大 小 以 及 所 有 
相关 属性 被 适当 地 缩放 (例如 关节 范围 .速度 设置 .质量 等 ) ,使 得 缩放 的 对 象 或 模型 可 以 正 
常 地 继续 操作 (但 是 以 不 同 的 比例 ) 。 

查看 /编辑 自 定义 数据 : 打开 一 个 允许 可 视 化 和 编辑 附加 到 对 象 的 自 定义 数据 的 对 话 
框 ,如 图 3-31 所 示 。 


Object Custom Data 


Attached custom data «o 
| [Au |] | Ger | 


Cose 


图 3-31 对 象 自 定义 数据 对 话 框 


附加 的 自 定义 数据 : 显示 附加 到 此 对 象 的 自 定义 数据 的 摘要 ,格式 为 : header number 
[data size]. header number [data size] 等 。 数 据 可 以 通过 API 调用 读 取 和 写 入 
(simAddObjectCustomData 和 simGetObjectCustomData)。 标 头号 是 自 定义 数据 和 /或 添 
加 它 的 开发 人 员 的 标识 符 。 如 果 要 添加 自 定义 数据 (作为 公司 或 个 人 ) ,请 始终 使 用 相同 的 
标题 (因为 只 有 我 们 才 知 道 哪 些 数据 类 型 存储 在 该 标题 下 )。 最 好 是 使 用 我 们 的 V-REP 副 
本 的 序列 号 ,或 高 随机 整数 值 。 否 则 ,我 们 可 能 会 与 其 他 开发 人 员 的 自 定义 数据 (可 能 与 我 
们 使 用 相同 的 标题 ) 冲 突 。 

添加 : 允许 向 对 象 添加 简单 的 自 定 义 数据 。 要 添加 的 数据 应 以 以 下 格式 写 入 命令 行 : 
头号 ,字符 串 或 头号 ,numberl ,number2,number3 等 。 不 带 “. ”的 数字 默认 为 整数 ,否则 为 
浮 点 数 。 以 小 端 方式 将 数字 添加 到 给 定 头 号 的 自 定 义 数据 缓冲 区 中 。 如 果 必 须 添加 /提取 
复杂 数据 ,请 使 用 API 调用 (simAddObjectCustomData 和 simGetObjectCustomData) 。 

清除 : 允许 从 对 象 中 删除 自 定义 数据 。 要 删除 的 数据 的 头 部 编号 应 写 人 其 右 侧 的 空 框 
中 。 如 果 必 须 添 加 /提取 复杂 数据 ,请 使 用 API 调用 (simAddObjectCustomData 和 
simGetObjectCustomData) 。 

装配 : 打开 一 个 对 话 框 , 可 以 指定 装配 工具 栏 按钮 在 装配 过 程 中 如 何 处 理 对 象 (如 果 对 
象 以 不 同 于 通过 装配 工具 栏 按钮 的 方式 装配 , 则 以 下 设置 不 会 有 影响 ) ,如 图 3-32 所 示 。 
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Assembling Behaviour 
[V] Object can have the child role. 
Requredmath vae — 0 


C Object can have the parent role 
Requredmathwale — 0 


When assembling, a speafic local transformation matrix wil be applied. 


Set matrix. 


Cose 


图 3-32 ”装配 行为 对 话 框 


对 象 可 以 具有 "* 子 ”角色 : 如 果 选 中 , 则 对 象 可 以 附加 到 另 一 个 对 象 ( 即 成 为 另 一 个 对 象 
的 子 对 象 ) ,但 前 提 是 所 需 的 匹配 值 与 其 父 对象 中 的 一 个 匹配 。 此 功能 对 于 设置 严 具 兼容 性 
标准 很 有 用 (例如 ,夹具 A 只 能 与 具有 类 型 A 的 工具 提示 的 机 器 人 连接 ) 。 

对 象 可 以 具有 “ 父 ” 角 色 : 如 果 选 中 , 则 另 一 个 对 象 可 以 附加 到 该 对 象 上 ( 即 成 为 另 一 个 
对 象 的 父 对 象 ) ,但 前 提 是 所 需 的 匹配 值 与 其 子 对 象 中 的 一 个 匹配 。 此 功能 对 于 设置 严 具 兼 
容 性 标准 很 有 用 (例如 ,夹具 A 只 能 与 具有 类 型 A 的 工具 提示 的 机 器 人 连接 )。 

设置 矩阵 : 当 对 象 被 组 装 时 ,将 使 用 特定 的 变换 矩阵 作为 其 新 的 局 部 变换 和 矩阵。 默认 
情况 下 , 抢 阵 是 单位 矩阵 ,但 可 以 通过 单 击 按钮 指定 特定 的 矩阵 。 该 特征 可 以 使 对 象 自动 并 
正确 地 定位 和 定向 到 其 新 的 父 对 象 上 (例如 ,为 了 使 抓 手 自动 正确 地 放置 在 机 器 人 的 工具 提 
示 处 )。 


3.4.3 常用 的 场景 对 象 一 一 形状 


形状 是 V-REP 中 的 基本 场景 对 象 ,它们 是 由 三 角形 面 组 成 的 刚性 网 格物 体 。 它 们 可 
以 导入 .导出 和 编辑 。 它 们 有 几 种 不 同 的 子 类 型 ; 

COD 简单 随机 形状 : 可 以 表示 任何 网 格 。 它 有 一 种 颜色 和 一 套 视 觉 属性 。 未 优化 动态 
碰撞 响应 ,不 推荐 用 于 动态 碰撞 响应 计算 (因为 非常 慢 和 不 稳定 ) 。 

(2) 复合 随机 形状 : 可 表示 任何 网 格 。 它 有 几 种 颜色 和 几 组 视觉 属性 。 未 优化 动态 碰 
撞 响 应 ,不 推荐 用 于 动态 碰撞 响应 计算 (因为 非常 慢 和 不 稳定 ) 。 

(3) 简单 凸 形 , 表示 具有 一 种 颜色 和 一 组 视觉 属性 的 凸 网 格 。 针 对 动态 碰撞 响应 计算 
进行 了 优化 (但 推荐 使 用 纯 形 状 ) 。 

(4) 复合 凸 形 : 表示 具有 几 种 颜色 和 一 组 视觉 属性 的 一 组 凸 网 格 。 针 对 动态 碰撞 响应 
计算 进行 优化 (但 推荐 使 用 纯 复 合 形状 ) 。 

C5) 纯 简单 形状 : 表示 原始 形状 (长 方 体 . 圆 柱 体 或 球体 ,根据 所 使 用 的 物理 引擎 具有 
额外 的 变化 ) 。 纯 简单 形状 (或 纯 复 合 形状 ) 适 合 于 动态 碰撞 响应 计算 ,因为 它 快 速 并 且 
稳定 。 


(60 纯 复 合 形状 : 表示 一 组 原始 形状 (长 方 体 、 圆 柱 体 或 球体 )。 纯 复合 形状 (或 纯 简 单 
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形状 适合 于 动态 碰撞 响应 计算 ,因为 它 快速 并 且 稳 定 。 

(7) 高 度 形状 : 可 以 将 地 形 表示 为 规则 网 格 ,其 中 只 有 高 度 变 化 。 该 形状 也 可 以 被 认 
为 是 纯 简单 形状 ,并 且 被 优化 用 于 动态 碰撞 响应 计算 。 

默认 情况 下 ,所 有 导入 的 形状 是 简单 的 形状 。 然 而 ,可 以 将 两 个 或 多 个 简单 形状 或 复合 
形状 分 组 或 取消 分 组 ,也 可 以 合并 简单 形状 。 在 这 种 情况 下 ,所 有 合成 元 素 的 视觉 属性 都 相 
同 。 也 可 以 分 割 简单 形状 ,这 取决 于 其 配置 : 分 割 算 法 将 提取 形状 的 每 个 不 同 元 素 。 如 果 
两 个 元 素 不 共享 任何 公共 边 , 则 两 个 元 素 不 同 。 

纯 形 状 主要 是 功能 形状 。 它 们 大 多 数 时 间 只 由 物理 引擎 使 用 ,因为 它们 比 非 纯 形 状 ( 例 
如 随机 或 凸 形 网 格 ) 具 有 更 好 和 更 快 的 执行 能 力 。 由 于 这 个 原因 , 纯 形 状 通常 隐藏 在 不 可 见 
层 (例如 层 9)。 

纯 简 单 形 状 也 可 以 分 组 ,然而 ,如 果 所 有 的 组 成 元 素 也 是 纯 的 , 则 所 得 到 的 复合 形状 将 
是 纯 的 。 合 并 纯 简 单 形状 将 导致 非 纯 简单 形状 。 

1. 形状 的 特点 

形状 是 可 碰撞 的 、 可 测量 的 、 可 检测 的 、 可 泻 染 的 和 可 切割 的 对 象 。 这 意味 着 形状 : 

。 可 用 于 对 其 他 可 碰撞 对 象 的 碰撞 检测 。 

。 可 用 于 与 其 他 可 测量 的 对 象 的 最 小 距离 计算 。 

可 以 由 接近 传感器 检测 。 
可 以 由 视觉 传感器 检测 。 

。 可 以 由 磨 机 切割 。 然 而 ,只 有 简单 的 非 纯 形 状 可 以 是 可 切割 的 。 

形状 的 可 碰撞 、 可 测量 .可 检测 、 可 呈现 和 可 裁 切 的 性 质 可 以 在 对 象 的 常见 属性 中 修改 。 
此 外 ,如 果 形 状 是 覆盖 它们 的 模型 的 一 部 分 ,那么 这 些 属 性 可 以 被 覆盖 。 

2. 形状 参考 坐标 系 和 边界 框 

形状 跟 每 个 对 象 一 样 ,具有 参考 坐标 系 和 边界 框 。 参 考 坐 标 系 或 坐标 坐标 系 ( 参 考 系 或 
坐标 系 ) 始 终 位 于 形状 的 几何 中 心 , 并 且 指示 从 中 计算 形状 的 位 置 和 取向 的 点 。 坐 标 系 具有 
三 个 轴 : X- Y-A 2- 轴 ,分别 对 应 于 红色 .绿色 和 蓝 色 箭头 。 形 状 的 边界 框 围绕 形状 的 参考 
坐标 系 居中 ,并 且 具 有 与 参考 坐标 系 相同 的 取向 (X、Y 和 2 轴 具 有 与 边界 框 的 边缘 相同 的 
取向 )。 边 界 框 完全 包围 形状 ,用户 可 以 从 4 种 不 同 的 方式 中 选择 以 定义 形状 的 参考 系 和 边 
界 框 方向 ( 纯 简 单 形状 和 高 度 场 形状 不 能 重新 定向 ) : 

(1) 与 世界 参考 坐标 系 对 齐 : 可 以 通过 依次 单 击 菜单 栏 编 辑 . 重 定向 边界 框 与 世界 
参考 坐标 系 打 开 。 当 单 击 该 项 目 (必须 预先 选择 形状 ) 时 ,将 计算 坐标 系 , 以 便 产 生 具 有 与 世 
界 参考 坐标 系 ( 即 绝对 坐标 系 ) 对 准 的 边缘 的 边界 框 。 

(2) 与 随机 形状 的 主轴 对 齐 : 可 以 通过 依次 单 击 菜单 栏 ` 编 辑 . 重 定向 边界 框 、 随 机 形 
状 的 主轴 打开 。 当 单 击 该 项 目 ( 必 须 预先 选择 形状 ) 时 ,将 计算 参考 系 ,以 便 在 随机 形状 周转 
产生 最 紧凑 的 边界 框 。 这 是 默认 的 计算 方法 。 

(3) 与 圆柱 形 主 轴 对 齐 : 可 以 通过 依次 单 击 菜单 栏 , 编 辑 、 重 定向 边界 框 、 圆 柱 形 主 轴 
打开 。 当 单 击 该 项 目 ( 必 须 预先 选择 形状 ) 时 ,将 以 圆柱 形状 计算 精确 的 参考 系 ,其 Z 轴 与 
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圆柱 的 旋转 轴 重 合 。 这 比 上 述 项 目 (与 随机 形状 的 主轴 对 准 ) 更 精确 ,但 需要 精确 限定 的 圆 
柱 形状 。 如 果 形 状 看 起 来 离 常规 和 精确 的 圆柱 太 远 ,操作 可 能 失败 。 

(4) 与 长 方 体形 状 的 主轴 对 齐 : 可 以 通过 依次 单 击 菜单 栏 、 编 辑 、 重 定向 边界 框 、 主 要 
长 方 体形 状 打 开 。 当 单 击 此 项 目 ( 必 须 预 先 选择 形状 ) 时 ,将 计算 与 立方 体 的 面 对 齐 的 长 方 
体形 状 的 精确 参考 系 。 这 比 上 述 项 目 ( 与 随机 形状 的 主轴 对 准 ) 更 精确 ,但 需要 精确 定义 的 
立方 体形 状 。 如 果 形 状 看 起 来 离 常规 和 精确 的 立方 体 太 远 , 则 操作 可 能 失败 。 

同样 地 ,也 可 以 在 几何 对 话 框 中 或 通过 API 修改 相对 于 其 形状 的 边界 框 方向 。 如 图 3-33 
所 示 ,说 明了 两 个 相同 形状 具有 不 同 参考 坐标 系 的 情况 。 


图 3-33 两 个 相同 的 形状 ,具有 不 同 的 参考 坐标 系 


3. 形状 的 性 质 

形状 属性 是 场景 对 象 属性 对 话 框 的 一 部 分 。 也 可 以 通过 双击 场景 层次 结构 中 的 对 象 图 
标 或 单 击 其 工具 栏 按钮 来 打开 对 话 框 。 

在 场景 对 象 性 质 对 话 框 ( 见 图 3-34) 中 , 单 击 形状 按钮 以 显示 形状 对 话 框 ( 仅 当 最 后 一 
个 选择 是 形状 时 , 才 会 显示 形状 按钮 )。 该 对 话 框 显 示 上 次 选择 的 形状 的 设置 和 参数 。 如 果 
选择 了 多 个 形状 , 则 可 以 将 一 些 参数 从 最 后 选择 的 形状 复制 到 其 他 所 选 形 状 ( 应 用 于 选择 
按钮 ) 。 

对 于 场景 对 象 性 质 的 说 明 如 下 。 

调整 颜色 : 允许 编辑 形状 的 颜色 。 

遮光 角 : 遗 光 角 是 区 分 各 个 面 的 角度 。 这 只 影响 形状 的 视觉 外 观 。 小 角度 使 得 形状 看 
起 来 清晰 ,具有 许多 边缘 ; 大 角度 使 得 形状 看 起 来 平滑 ,并 且 具 有 较 少 的 边缘 。 

显示 边缘 : 以 黑色 显示 边缘 。 显 示 的 边缘 将 取决 于 指定 的 角度 。 如 果 选 中 隐藏 边框 ， 
则 不 共享 多 个 三 角形 的 边 将 被 隐藏 。 

背面 剔除 : 构成 一 个 形状 的 每 个 三 角形 都 有 一 个 内 表面 和 一 个 外 表面 。 当 启用 背面 易 
除 时 ,不 会 显示 内 部 面 。 这 是 对 封闭 形状 和 透明 形状 有 用 的 参数 。 

反 转 面 : 这 将 翻转 所 有 三 角形 。 内 表面 变 成 外 表面 ,外 表面 变 成 内 表面 。 凸 形 形 状 将 
变 为 非 凸 形 , 纯 形 状 除外 。 

线 框 : 如 果 选 中 , 则 通过 相机 查看 时 ,形状 将 始终 显示 为 线 框 。 

调整 纹理 : 打开 所 选 形状 的 纹理 对 话 框 。 当 形状 与 纹理 相关 联 时 ,将 以 纹理 方式 显示 。 
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Scene Object Properties B 
se Omm | 
Visual properties 
Colors. 
o Adestck 0 
Apply to selecbon. 
Other propertes. 
Shadngange [deg] 20.0 
V Show edges with angie [deg] 20.0 Hidden border 
Backface ang | Invert faces | 
Wreframe. 


图 3-34 场景 对 象 性 质 对 话 框 


快速 纹理 (选择 ) : 将 三 维 映射 纹理 应 用 于 所 有 选 定 的 形状 。 这 对 于 用 作 “ 污 垢 ”的 无 缝 
纹理 特别 有 用 ,以 使 对 象 看 起 来 更 逼真 。 

清除 纹理 (选择 ): 从 所 有 选 定 的 形状 中 删除 纹理 。 

查看 /修改 几何 : 打开 所 选 形状 的 形状 几何 对 话 框 。 它 允许 调整 网 格 的 各 种 参数 。 

显示 动态 属性 对 话 框 : 切换 形状 动力 学 属性 对 话 框 。 形 状 动态 对 话 框 允许 调整 形状 的 
动力 学 属性 。 

以 上 的 一 些 参 数 仅 适用 于 简单 形状 。 选 择 复合 形状 时 ,可 以 通过 切换 到 复合 形状 的 形 
状 编辑 模式 来 编辑 其 视觉 属性 。 当 然 也 可 以 取消 组 合 它 , 以 便 单独 编辑 其 组 件 。 

1) 形状 的 动力 学 特性 

形状 动力 学 对 话 框 是 形状 属性 的 一 部 分 。 对 话 框 显示 上 次 选择 的 形状 的 动态 设置 和 参 
数 。 如 果 未 选择 对 象 , 则 对 话 框 处 于 非 活动 状态 。 如 果 选 择 了 多 个 形状 , 则 可 以 将 一 些 参数 
从 最 后 选择 的 形状 复制 到 其 他 所 选 形状 (应 用 于 选择 按钮 )。 形 状 的 动力 学 特性 对 话 框 如 
图 3-35 所 示 ,说 明 如 下 。 

刚体 是 可 响应 的 : 如 果 启 用 , 则 形状 将 产生 与 其 他 可 响应 形状 的 碰撞 反应 ,但 前 提 是 相 
应 的 可 应 答 掩 码 重 又 。 更 多 细节 参见 设计 动态 仿真 部 分 。 

可 响应 掩 码 : 指示 何 时 生成 冲突 响应 (但 是 需要 启用 可 响应 项 目 )。 掩 码 由 两 个 8 位 值 
组 成 。 如 果 两 个 磁 撞 形状 共享 它们 的 任何 父 对 象 (直接 或 间接 ), 则 使 用 局 部 掩 码 ,否则 使 用 
全 局 掩 码 。 如 果 两 个 形状 的 AND 组 合 掩 码 ( 局 部 或 全 局 ) 不 为 零 , 则 将 生成 冲突 响应 。 

材质 : 所 选 形状 的 所 选材 质 。 材 料 属性 将 所 有 动态 引擎 的 特定 属性 (如 摩擦 ,恢复 等 ) 
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Rigid Body Dynamic Properties 
[V] Body is respondabie 
Locirespondeblemesk OOOO AAMA 


Globalrespondablemask HAAA AAAA 
id [di 7) 


[V] Body is dynamic 

C Startin sleep mode [C Set to dynamic if gets parent 

._ Compute mass & inertia properbes for selected convex shapes —- 
Mass 


Mass[kg] [30000401 | 


swa [umen] (eremi E 
Yim | 1.000€01 一 一 
Z|m^2 | Lo00e01 FAUR. ee 


Pos./orient. of inertia frame & COM relative to shape frame 
xim] 40.000400 
YI。 [0.000et00| 


Z 罗 |40.000e+00 


图 3-35 ”形状 的 动力 学 特性 对 话 框 


组 合 在 一 起 ,并 可 与 其 他 几 种 形状 共享 。 可 以 通过 材料 属性 查看 和 修改 (编辑 ) 材 料 。 

刚体 是 动态 的 : 启用 时 ,形状 的 位 置 和 方向 将 在 动力 学 仿真 中 受到 影响 。 

在 睡眠 模式 下 开始 : 动态 仿真 的 可 响应 形状 可 以 以 睡眠 模式 开始 ,在 这 种 情况 下 ,其 不 
会 对 约束 (例如 重力 ) 做 出 反应 ,直到 其 首先 与 另 一 可 响应 形状 碰撞 。 

设置 为 动态 如 果 获 得 父 级: 启用 并 且 形 状 附加 到 另 一 个 对 象 时 ,形状 将 自动 变 为 动态 。 
这 对 于 在 独立 时 应 该 是 静态 的 模型 基础 是 有 用 的 ,但 是 当 与 另 一 模型 /对 象 结合 时 , 变 为 动 
态 的 。 例 如 ,自身 操作 的 机 器 人 操纵 器 通常 具有 其 基部 静止 ,但 是 当 附 接 到 车 辆 时 ,应 该 变 
为 动态 。 

计算 所 选 形状 的 质量 和 惯性 属性 : 通过 单 击 此 按钮 ,可 以 基于 材料 均匀 密度 自动 计算 
所 选 凸 形状 的 质量 和 惯性 属性 。 

质量 : 形状 的 质量 。 所 选 形状 可 以 使 用 M—M x 2( 用 于 选择 ) 和 M= M/2( 用 于 选择 ) 
按钮 ,使 其 质量 增 大 或 减 小 两 倍 。 这 便于 通过 试 错 法 快速 找到 稳定 的 仿真 参数 。 

主要 转动 惯量 /质量 : 无 质量 ( 即 除 以 形状 的 质量 ) 主 转动 惯量 。 所 选 形状 可 以 使 用 
ISI» 2( 用 于 选择 ) 和 [一 IJ/2( 用 于 选择 ) 按 钮 ,使 其 无 质量 惯量 值 增 至 两 倍 或 减少 一 半 。 
这 便于 通过 试 错 法 快速 找到 稳定 的 仿真 参数 。 

Pos. /orient. 的 惯性 坐标 系 和 COM 相对 于 形状 坐标 系 : 惯性 坐标 系 的 配置 和 相对 于 形 
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状 的 参考 坐标 系 表示 的 质心 的 配置 。 

设置 惯性 矩阵 和 COM 相对 于 绝对 坐标 系 : 打开 惯性 矩阵 对 话 框 ,允许 指定 相对 于 绝对 
参考 坐标 系 的 惯性 属性 。 对 象 的 自 定 义 数据 对 话 框 如 图 3-36 Bros ,说 明 如 下 。 

惯性 矩阵 除 以 质量 : 惯性 矩阵 或 张 量 。 值 是 无 质量 的 ( 即 除 以 形状 的 质量 )。 和 矩阵 必须 
相对 于 形状 的 质心 表示 ( 即 矩阵 是 对 称 的 ) 。 

质心 位 置 : 质心 的 位 置 。 

应 用 于 选 定 的 形状 : 选中 时 ,所 有 选 定 的 形状 将 相对 于 绝对 参考 系 具有 相同 的 惯性 属 
性 ( 即 质心 和 惯性 矩阵 的 所 有 中 心 将 重合 )。 

2) 形状 的 材料 特性 

可 以 通过 形状 动力 学 特性 对 话 框 访问 材料 特性 ( 即 与 形状 相关 的 动力 学 引擎 特性 ) 。 

3) 形状 的 几何 特征 对 话 框 

形状 几何 对 话 框 如 图 3-37 所 示 , 它 是 形状 属性 的 一 部 分 。 该 对 话 框 允许 我 们 查看 与 形 
状 相关 联 的 几何 图 形 , 并 在 一 定 程度 上 对 其 进行 修改 。 它 显示 顶点 和 三 角形 的 数量 ,以 及 形 
状 边界 框 的 大 小 。 网 格 的 顶点 和 三 角形 的 数量 与 泻 染 和 计算 时 间 直 接 相关 (例如 ,在 冲突 检 
测 或 距离 计算 期 间 ) ,并 且 网 格 具 有 的 顶点 /三 角形 越 多 ,仿真 或 场景 显示 越 慢 。 虽 然 对 象 公 
共 属 性 允许 缩放 对 象 (包括 形状 ) ,但 是 它 始 终 保持 比例 相同 ( 沿 对 象 的 xz、y 或 轴 的 缩放 
比例 相同 )。 此 限制 不 适用 于 几何 对 话 框 ,我 们 甚至 可 以 通过 指定 负 缩 放 因子 来 沿 着 其 中 一 
个 轴 翻 转 网 格 。 但 是 请 记 住 ,一 些 纯 简单 的 形状 和 纯 复 合 形状 对 非 均 匀 缩 放 有 限制 。 除 此 
之 外 , 纯 形状 不 能 翻转 。 
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图 3-36 对象 的 自 定义 数据 对 话 框 图 3-37 形状 的 几何 特征 对 话 框 
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此 外 ,形状 几何 对 话 框 允许 修改 形状 的 边界 框 方向 ,这 应 该 只 在 非常 特殊 的 情况 下 使 
。 修 改 形 状 边界 框 方向 的 首选 方法 是 先 选中 某 个 形状 ,然后 依次 选择 菜单 栏 ` 编 辑 `. 边 界 
框 对 齐 、 将 所 选 形状 的 坐标 框 与 主轴 对 齐 或 依次 选择 菜单 栏 ` 编 辑 ` 边 界 框 对 齐 、 将 所 选 形状 
的 坐标 坐标 系 与 世界 对 齐 。 

4. 形状 的 编辑 模式 

在 V-REP 中 , 当 需 要 使 用 到 特定 形状 时 ,最 佳 选 择 是 在 CAD 应 用 程序 (例如 
AutoCAD,3D Studio Max 等 ) 中 绘制 它们 ,然后 导入 它们 。 当 外 部 CAD 应 用 程序 不 可 用 或 
只 需要 简单 形状 时 ,用 户 可 以 创建 基本 形状 ,然后 在 V-REP 支持 的 三 种 形状 编辑 模式 的 其 
中 一 种 模式 下 自 定义 创建 的 形状 : 

(1) 三 角形 编辑 模式 : 在 此 模式 下 ,构成 形状 的 单个 三 角形 是 可 见 的 ,可 以 操作 或 
提取 。 

(2) 顶点 编辑 模式 : 在 此 模式 下 ,组 成 形状 的 单个 顶点 是 可 见 的 ,可 以 操作 或 提取 。 

(3) 边缘 编辑 模式 : 在 此 模式 下 ,构成 形状 的 单个 边缘 可 见 , 并 且 可 以 进行 操作 或 
提取 。 

在 输入 上 述 编辑 模式 之 前 , 纯 形状 将 转换 为 常规 形状 。 复 合 形状 不 能 直接 编辑 , 它 将 首 
先 被 取消 分 组 。 或 者 通过 复合 形状 的 编辑 模式 编辑 其 合成 元 素 的 视觉 参数 。 通 过 单 击 相 应 
的 工具 栏 按钮 可 访问 不 同 的 编辑 模式 。 

确保 在 进入 形状 编辑 模式 之 前 选择 了 一 个 形状 对 象 。 在 形状 编辑 期 间 ,无 法 选择 对 象 ， 
也 无 法 开始 仿真 。 一 旦 编辑 完成 , 单 击 形状 编辑 模式 工具 栏 按钮 完成 编辑 模式 。 当 对 形状 
应 用 更 改 时 ,V-REP 将 确保 修改 后 的 形状 一 致 .并 删除 未 使 用 的 顶点 和 合并 彼此 靠近 的 顶 
点 等 。 可 以 在 用 户 的 顶点 /三 角形 验证 设置 中 设置 确切 的 行为 设置 对 话 框 。 

可 以 从 一 种 编辑 模式 切换 到 另 一 种 编辑 模式 ,这 允许 我 们 实现 特殊 操作 。 例 如 ,如 果 要 
选择 和 删除 圆柱 体 表面 的 所 有 三 角形 ,而 不 是 在 三 角形 编辑 模式 下 单独 选择 它们 ,请 在 顶点 
编辑 模式 下 执行 所 有 上 顶点 的 移 位 选择 ,然后 切换 到 三 角 编 辑 模 式 ,最 后 按 下 删除 键 。 

1) 三 角 编 辑 模 式 

可 以 通过 单 击 相应 的 工具 栏 按钮 访问 三 角形 编辑 模式 。 

上 述 工具 栏 按钮 仅 在 选择 形状 时 有 效 。 如 果 最 后 选择 的 形状 不 是 简单 形状 ,而 是 复合 
形状 , 则 将 激活 复合 形状 的 编辑 模式 。 在 三 角形 编辑 模式 中 ,组 成 形状 的 所 有 三 角形 将 分 别 
显示 。 三 角形 有 两 个 面 , 即 正面 和 背面 。 正 面 以 蓝 色 显 示 , 而 背面 以 红色 显示 。 当 选择 三 角 
形 ( 使 用 与 选择 对 象 相同 的 步骤 ) 时 ,它们 将 以 黄色 显示 ,最 后 一 个 选 定 的 三 角形 以 白色 显 
示 。 支 持 使 用 快捷 方式 进行 复制 、. 剪 切 、 粘 贴 . 删 除 ( 分 别 对 应 于 快捷 键 Ctrl 十 c,Ctrl 十 x， 
Ctrl 十 v,Delete) 。 取 消 选择 可 以 使 用 Esc 键 ,或 者 使 用 取消 选择 工具 栏 按钮 ,又 或 者 通过 
Ctrl 单 击 场景 的 空白 区 域 。 移 位 选择 将 选择 所 选区 域 下 所 有 的 三 角形 以 及 隐藏 的 三 角形 
(如 果 只 想 通过 移 位 选择 可 见 的 三 角形 ,请 在 按 住 Shift 键 的 同时 按 住 Ctrl 键 )。 在 三 角形 
编辑 模式 中 ,通常 显示 场景 层次 的 窗口 部 分 用 于 显示 正 被 编辑 为 列表 的 形状 的 三 角形 。 可 
以 使 用 鼠标 选择 列表 中 的 项 目 作为 层次 结构 窗口 中 的 对 象 。 


第 3 章 “V-REP 在 机 器 人 仿真 中 的 应 用 [e 241 


进入 三 角 编 辑 模式 后 ,编辑 菜单 项 将 变 为 特定 于 三 角形 的 编辑 模式 ,并 将 显示 以 下 对 话 
框 , 如 图 3-38 所 示 。 


Shape Edition 
Tnange edt mode Vertexedtmode | 


图 3-38 三 角 编辑 模式 对 话 框 


三 角 / 顶 点 / 边 编辑 模式 : 当前 激活 的 编辑 模式 。 用 户 可 以 通过 这 些 按钮 从 一 种 编辑 模 
式 切换 到 其 他 编辑 模式 (所 选项 目 将 被 保留 ) 。 

清除 选择 : 取消 三 角形 的 选择 。 

反 转 选择 : 反 转 三 角形 的 选择 状态 。 

提取 形状 : 将 基于 当前 选 定 的 三 角形 创建 一 个 简单 的 形状 。 形 状 将 添加 到 场景 ,但 可 
能 不 可 见 (编辑 模式 下 的 形状 和 新 创建 的 形状 在 位 置 和 方向 上 重合 ,或 第 一 个 可 见 性 层 被 禁 
用 )。 将 保留 三 角形 选择 。 

提取 立方 体 : 将 基于 当前 选 定 的 三 角形 创建 矩形 ( 纯 ) 简 单 形状 (将 以 与 包含 所 选 三 角 
形 的 最 小 边界 框 相同 的 方式 定向 新 立方 体 ) ,将 保留 三 角形 选择 。 当 需要 以 简化 的 方式 对 复 
杂 的 网 格 形状 进行 建 模 ( 例 如 有 效 的 动力 学 仿真 ) 时 ,该 操作 是 有 用 的 。 

提取 柱 : 将 基于 当前 选 定 的 三 角形 创建 圆柱 形 ( 纯 ) 简 单 形状 (将 以 与 包含 所 选 三 角形 
的 最 小 边界 框 相同 的 方式 定向 新 柱 形 )。 将 保留 三 角形 选择 。 当 需要 以 简化 的 方式 对 复杂 
的 网 格 形状 进行 建 模 (例如 有 效 的 动力 学 仿真 ) 时 .该 操作 是 有 用 的 。 

提取 球体 : 将 基于 当前 选择 的 三 角形 创建 一 个 球形 ( 纯 ) 简 单 形状 (将 以 与 包含 所 选 三 
角形 的 最 小 边界 框 相同 的 方式 定向 新 球体 )。 将 保留 三 角形 选择 。 当 需要 以 简化 的 方式 对 
复杂 的 网 格 形状 进行 建 模 (例如 有 效 的 动力 学 仿真 ) 时 ,该 操作 是 有 用 的 。 

翻转 : 翻转 所 选 三 角形 的 边 ( 从 红色 到 蓝 色 . 从 蓝 色 到 红色 ) 。 

细 分 最 大 的 三 角形 : 这 将 划分 大 的 三 角形 。 除 非 想 减 小 三 角形 的 大 小 ,以 获得 更 大 差 
别 的 大 表面 照明 .其 他 情况 下 不 建议 减少 三 角形 的 大 小 。 相 反 ,我 们 可 以 在 环境 对 话 框 中 调 
整 计算 结构 的 三 角形 大 小 。 

三 角形 可 以 直接 使 用 鼠标 转换 方向 ,使 用 工具 栏 的 对 象 /项 目 转换 按钮 将 在 垂直 平面 中 
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选中 的 三 角形 转换 到 视图 方向 。 


2 


) 顶点 编辑 模式 


可 以 通过 单 击 相 应 的 工具 栏 按钮 访问 顶点 编辑 模式 。 该 工具 栏 按钮 仅 在 选择 形状 后 有 


A. ou 


0 果 最 后 选择 的 形状 不 是 简单 形状 ,而 是 复合 形状 , 则 将 激活 复合 形状 的 编辑 模式 。 在 


顶点 编辑 模式 中 ,组 成 形状 的 所 有 项 点 单独 显示 为 红色 。 当 选择 顶点 (使 用 与 选择 对 象 相同 
的 步骤 ) 时 ,它们 将 以 黄色 显示 ,最 后 选择 的 顶点 以 白色 显示 。 顶 点 支持 使 用 快捷 方式 来 复 


制 、 剪 


切 、 粘 贴 .删除 (分 别 对 应 于 快捷 键 Ctrl 十 c,Ctrl 十 x,Ctrl 十 v, Delete)。 取 消 选择 可 以 


使 用 Esc 键 ,或 者 使 用 取消 选择 工具 栏 按 钮 ,又 或 者 通过 Ctrl 单 击 场景 的 空白 区 域 。 移 位 
选择 将 选中 所 选区 域 下 的 所 有 顶点 ,以 及 隐藏 的 项 点 (如 果 只 想 通 过 移 位 选择 可 见 的 顶点 ， 
则 在 按 住 Shift 键 的 同时 按 住 Ctrl 键 )。 在 顶点 编辑 模式 中 ,通常 显示 场景 层次 的 窗口 部 分 


用 于 


E 被 编辑 的 形状 的 顶点 显示 为 列表 。 可 以 使 用 鼠标 选择 列表 中 的 项 目 作 为 层次 结构 窗 


口中 的 对 象 。 
进入 顶点 编辑 模式 后 ,编辑 菜单 项 将 变 为 特定 于 顶点 的 编辑 模式 。 顶 点 编辑 模式 对 话 
框 如 图 3-39 所 示 。 
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图 3-39 ”顶点 编辑 模式 对 话 框 


三 角 / 顶 点 / 边 编辑 模式 : 当前 激活 的 编辑 模式 。 用 户 可 以 通过 这 些 按 钮 从 一 种 编辑 模 
式 切换 到 其 他 编辑 模式 (所 选项 目 将 被 保留 ) 。 

显示 隐藏 顶点 : 选择 时 ,将 显示 隐藏 顶点。 

清除 选择 : 清除 顶点 的 选择 。 

反 转 选择 : 反 转 顶点 的 选择 状态 。 

插入 三 角形 : 将 在 所 选 顶点 之 间 插 和 三角形。 选择 顶点 的 顺序 很 重要 。 

插入 三 角 风 扇 : 将 在 所 选 顶点 之 间 插 入 三 角形 风扇 。 选 择 顶 点 的 顺序 很 重要 。 最 后 选 
择 的 顶点 将 是 新 插入 的 三 角形 之 间 的 公共 顶点 。 

Make Dummies: 将 根据 当前 选择 的 顶点 创建 虚拟 对 象 。 当 需要 设置 形状 的 精确 坐标 


系 时 ， 


这 是 一 个 有 用 的 特征 。 基 于 顶点 创建 虚拟 对 象 ,离开 编辑 模式 并 将 形状 附加 到 虚拟 对 
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象 上 (虚拟 变 为 父 对 象 )。 然 后 可 以 通过 虚拟 对 象 操纵 形状 的 位 置 。 

顶点 可 以 通过 坐标 和 变换 对 话 框 精确 定位 。 它 们 也 可 以 直接 使 用 鼠标 转换 ,使 用 工具 
栏 的 对 象 /项 目 转换 按钮 将 垂直 平面 中 选 定 的 顶点 转换 为 视图 方向 。 

3) 边缘 编辑 模式 

可 以 通过 单 击 相应 的 工具 栏 按钮 访问 边缘 编辑 模式 。 该 工具 栏 按钮 仅 在 选择 形状 后 有 
效 。 如 果 最 后 选择 的 形状 不 是 简单 形状 ,而 是 复合 形状 , 则 将 激活 复合 形状 的 编辑 模式 。 在 
边缘 编辑 模式 中 ,构成 形状 的 所 有 边缘 单独 显示 为 红色 。 当 选择 边缘 (使 用 与 选择 对 象 相同 
的 步骤 ) 时 ,它们 将 显示 为 黄色 ,最 后 一 个 选 定 的 边缘 显示 为 白色 。 可 以 使 用 Delete 键 删除 
边缘 ,但 不 能 复制 . 剪 切 或 粘贴 。 取 消 选 择 可 以 使 用 Esc 键 ,或 者 使 用 取消 选择 工具 栏 按钮 ， 
又 或 者 通过 Ctrl 单 击 场景 的 空白 区 域 。 移 位 选择 将 选中 所 选区 域 下 的 所 有 边缘 ,以 及 隐藏 
边缘 (如 果 只 希望 通过 移 位 选择 选择 可 见 边缘 ,请 在 按 住 Shift 键 的 同时 按 住 Ctrl 键 )。 在 
边缘 编辑 模式 中 ,通常 显示 场景 层次 的 窗口 部 分 用 于 将 正 被 编辑 的 形状 的 边缘 显示 为 列表 。 
可 以 使 用 鼠标 选择 列表 中 的 项 目 作为 层次 结构 窗口 中 的 对 象 。 

进入 边缘 编辑 模式 后 ,编辑 菜单 项 将 变 为 特定 于 边缘 的 编辑 模式 。 边 缘 编 辑 模式 对 话 
框 如 图 3-40 所 示 。 
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图 3-40 边缘 编辑 模式 对 话 框 


三 角 / 顶 点 / 边 编辑 模式 : 当前 激活 的 编辑 模式 。 用 户 可 以 通过 这 些 按 钮 从 一 种 编辑 模 
式 切换 到 其 他 编辑 模式 (所 选项 目 将 被 保留 ) 。 

显示 隐藏 边缘 : 选择 时 ,也 将 显示 隐藏 的 边缘 。 

自动 边缘 跟随 : 当 该 选项 被 选中 并 且 有 边缘 被 选择 时 .该 边缘 将 被 跟随 ,直到 创建 循环 
或 者 边缘 突然 改变 方向 (最 大 直径 变化 角度 ) 或 消失 (最 大 边缘 角度 ) 。 当 我 们 不 仅 要 提取 局 
部 边缘 (三 角形 的 一 侧 ), 而 且 需 要 提取 全 局 边缘 (形状 的 边缘 ) 时 ,这 是 一 个 有 用 的 功能 。 边 
缘 只 在 一 个 方向 上 被 跟随 。 

清除 选择 : 清除 边缘 的 选择 。 

反 转 选择 : 反 转 边缘 的 选择 状态 。 
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提取 路 径 : 将 基于 当前 选 定 的 边缘 创建 路 径 对 象 。 每 个 边 表示 路 径 中 两 个 连续 控制 点 
之 间 的 段 (Bezier 插值 被 关闭 ,每 个 控制 点 的 贝 塞 尔 点 计数 为 1) 。 该 路 径 提 取 功 能 在 许多 
不 同 的 情况 下 非常 有 用 (例如 ,使 焊接 机 器 人 跟随 物体 的 边缘 ,或 者 使 轨道 型 物体 产生 其 相 
应 的 路 径 ) 。 

边缘 可 以 直接 通过 单 击 工具 栏 的 对 象 /项 目 转换 按钮 进行 转换 ,可 以 将 垂直 平面 中 选中 
的 边缘 转换 到 视图 方向 。 

4) 复合 形状 编辑 模式 

上 述 工具 栏 按钮 仅 在 选择 形状 时 有 效 。 如 果 最 后 选择 的 形状 不 是 复合 形状 ,而 是 简单 
的 形状 , 则 将 激活 三 角形 编辑 模式 。 在 复合 形状 的 编辑 模式 中 ,可 以 编辑 形状 组 件 的 单个 视 
觉 参 数 。 形 状 的 组 件 可 以 选择 层次 结构 窗口 。 

在 进入 复合 形状 的 编辑 模式 后 ,将 显示 复合 形状 的 编辑 模式 对 话 框 ,如 图 3-41 所 示 。 


Edition of Grouped Shapes 


Colors | texture 
Adjust color Adjust texture 


Other propertes 
Shadngange[deg] 20.0 
Y Show edges with angle [deg] 
 Badkface ading 
Wireframe 


图 3-41 复合 形状 的 编辑 模式 对 话 框 


调整 颜色 : 允许 编辑 形状 组 件 的 颜色 。 

调整 纹理 : 打开 所 选 形状 组 件 的 纹理 对 话 框 。 

遮光 角 : 遗 光 角 是 区 分 各 个 面 的 角度 。 小 角度 使 得 形状 看 起 来 清晰 ,具有 许多 边缘 ,大 
角度 使 得 形状 看 起 来 平滑 并 且 具 有 较 少 的 边缘 。 

显示 边缘 : 以 黑色 显示 边缘 。 显 示 的 边缘 将 取决 于 指定 的 角度 。 如 果 选 中 隐藏 边框 ， 
则 不 共享 多 个 三 角形 的 边 将 被 隐藏 。 

背面 剔除 : 构成 一 个 形状 的 每 个 三 角形 都 有 一 个 内 表面 和 一 个 外 表面 。 当 启用 背面 易 
除 时 ,不 会 显示 内 部 面 。 这 是 封闭 形状 和 透明 形状 的 有 用 参数 。 

线 框 : 如 果 选 中 , 则 通过 相机 查看 时 ,形状 将 始终 显示 为 线 框 。 


3.4.4 常用 的 场景 对 象 一 一 关节 


关节 是 具有 至 少 一 个 内 在 自由 度 (自由 度 ) 的 对 象 。 关 节 用 于 构建 机 构 和 移动 物体 。 通 
过 依次 单 击 菜单 栏 、 添 加 、 关 节 可 将 关节 添加 到 场景 中 。 与 另 一 个 对 象 相 比 ,关节 具有 两 个 
参考 帧 ( 仅 在 选择 关节 时 可 见 )。 第 一 个 是 固定 的 常规 参考 系 ,其 他 对 象 也 有 ; 第 二 参考 帧 
不 是 固定 的 ,并 且 将 根据 定义 其 配置 的 关节 位 置 ( 或 关节 值 ) 相 对 于 第 一 参考 帧 移动 。 
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1. 关节 类 型 
V-REP 有 4 种 类 型 的 关节 ,如 图 3-42 所 示 。 


3 | € 


图 3-42 旋转 关节 、 棱 柱 形 关节 、 螺 杆 和 球形 关节 


旋转 关节 : 旋转 关节 具有 1 个 自由 度 , 并 用 于 描述 对 象 之 间 的 旋转 运动 。 它 们 的 配置 
由 一 个 值 定义 ,这 个 值 代表 的 是 它们 在 第 一 参考 系 下 的 x 轴 的 旋转 量 。 它 们 可 用 作 被 动 关 
节 或 主动 关节 (电机 )。 

棱柱 关节 : 棱柱 关节 有 一 个 自由 度 , 用 于 描述 物体 之 间 的 平移 运动 。 它 们 的 配置 由 一 
个 值 定义 ,这 个 值 代表 的 是 沿 着 第 一 参考 系 的 = 轴 的 平移 量 。 它 们 可 用 作 被 动 关节 或 主动 
关节 (电机 )。 

螺钉 : 螺钉 可 以 看 作 是 旋转 关节 和 棱柱 关节 (具有 连 杆 值 ) 的 组 合 ,具有 1 个 自由 度 , 用 
于 描述 类 似 于 螺钉 的 运动 。 螺 距 参数 定义 了 给 定 旋转 量 下 的 平移 量 。 螺 杆 构造 由 表示 围绕 
其 第 一 参考 系 的 = 轴 的 旋转 量 的 一 个 值 限 定 。 螺 钉 可 用 作 被 动 关节 或 主动 关节 (电机 ) 。 

球形 关节 : 球形 关节 具有 3 个 自由 度 , 用 于 描述 物体 之 间 的 旋转 和 运动。 它们 的 配置 由 
表示 围绕 其 第 一 参考 系 的 +、y 和 < 轴 的 旋转 量 的 三 个 值 定义 。 定 义 球形 关节 构造 的 三 个 
值 被 指定 为 欧 拉 角 。 在 一 些 情况 下 ,球形 关节 可 以 被 认为 是 3 个 旋转 关节 组 成 ,它们 在 层级 
链 中 彼此 联系 ,在 空间 中 正 交 。 然 而 :这 种 类 比 只 有 当 任意 一 个 旋转 关节 保持 一 个 不 同 于 其 
他 两 个 关节 的 方向 时 是 有 用 的 。 事 实 上 ,如果 两 个 关节 接近 一 致 ,特殊 的 情况 可 能 出 现 ,机 
械 装 置 可 能 会 失去 一 个 自由 度 。 这 在 内 部 结构 避免 这 种 情况 的 球形 关节 不 会 发 生 。 球 形 关 
节 总 是 被 动 关节 ,不 能 用 作 电 机 。 

关节 用 于 允许 其 父 对 象 与 其 子 对 象 之 间 的 相对 运动 。 当 在 关节 和 对 象 之 间 建 立 父子 关 
系 时 ,对 象 附 接 到 关节 的 第 二 参考 系 , 因 此 ,关节 的 配置 (内 在 位 置 ) 的 改变 将 直接 反映 到 子 
对 象 上 。 可 以 通过 依次 单 击 菜单 栏 , 添 加 、 关 节 以 将 新 关节 添加 到 场景 。 

2. 关节 模式 

关节 可 以 是 以 下 模式 之 一 。 

被 动 模式 : 在 这 种 模式 下 ,关节 不 被 直接 控制 ,并 且 将 用 作 固定 构件 。 然 而 ,用 户 可 以 
通过 适当 的 API 函数 调用 (例如 simSetJointPositon 或 simSetSphericalJointMatrix) 来 改变 
关节 的 位 置 。 

反 运 动 学 模式 : 在 这 种 模式 下 .关节 作为 被 动 关节 ,但 在 逆 运 动 学 计算 或 几何 约束 求解 


246 十 | 机 器 人 仿真 与 编程 技术 


器 计算 中 使 用 (调整 ) 。 

从 属 模式 : 在 此 模式 下 ,关节 位 置 通过 线性 方程 直接 连接 (依赖 ) 到 另 一 个 关节 位 置 。 

运动 模式 : 此 模式 已 弃 用 ,不 应 再 使 用 。 使 用 被 动 模式 和 子 脚 本 适当 地 更 新 关节 可 以 
获得 类 似 和 更 灵活 的 行为 。 

力矩 或 力 模 式 : 在 这 种 模式 下 , 当 且 仅 当 动态 启用 时 ,动力 学 模块 才 会 仿真 关节 。 当 动 
态 启 用 时 ,关节 可 以 受到 力 /力矩 .速度 或 位 置信 号 的 控制 而 自由 运动 。 螺 钉 不 能 在 力矩 或 
力 模式 下 操作 (但 是 可 以 通过 编程 连接 旋转 关节 和 棱柱 关节 获得 类 似 的 行为 ) ,球形 关节 只 
能 在 力矩 或 力 模式 下 自由 运动 。 

当 关节 马达 被 禁用 时 ,关节 是 自由 的 ,并 且 仅 受 其 自身 的 约束 。 

当 关节 电机 启用 并 且 控 制 回路 被 禁用 时 ,在 给 定 其 能 够 输送 的 最 大 力矩 / 力 的 情况 下 ， 
关节 将 尝试 达到 期 望 的 目标 速度 。 当 最 大 力矩 / 力 非常 高 时 ,瞬时 达到 目标 速度 ,于 是 关节 
在 速度 控制 中 运动 ,否则 其 以 指定 的 力矩 / 力 运 动 ,直到 达到 期 望 的 目标 速度 (力矩 / 力 
控制 ) 。 

当 关节 电机 启用 并 且 控 制 回路 启用 时 ,用 户 有 3 种 控制 模式 : 

自 定义 控件 : 关节 控件 调用 脚本 将 负责 控制 关节 的 动态 行为 ,允许 我 们 使 用 任何 我 们 
能 够 想到 的 算法 控制 关节 。 

位 置 控制 (PID): 通过 PID 控制 器 控制 关节 的 位 置 ,通过 以 下 方式 调节 关节 速度 (At 分 
频 器 使 控制 器 不 受 选 定 的 控制 器 的 时 间 步 长 影响 ) 。 

弹 筑 减 震 器 模式 : 关节 将 通过 力 / 力 矩 调节 作用 使 其 类 似 于 弹簧 减 震 器 系统 。 

当 关节 处 于 被 动 模式 . 逆 运 动 学 模式 或 相关 模式 时 ,其 也 可 以 以 混合 方式 操作 : 混合 操 
作 人 允许 关节 以 常规 方式 操作 ,但 是 ,在 动态 计算 之 前 ,当前 关节 位 置 将 被 复制 到 目标 关节 位 
置 , 然 后 ,在 动力 学 计算 期 间 ,关节 将 作为 位 置 控 制 中 的 电机 处 理 ( 如 果 且 仅 当 其 被 动态 启用 
时 (参考 关于 设计 动态 仿真 的 部 分 以 获得 更 多 信息 ))。 例 如 ,该 特征 允许 通过 简单 地 指定 期 
望 的 脚 的 位 置 (作为 逆 运 动 学 任务 ) 来 控制 类 人 机 器 人 的 腿 ; 则 通过 相应 的 计算 得 出 的 关节 
位 置 将 被 应 用 为 腿 部 动态 运动 的 位 置 控制 值 。 

3. 关节 控制 器 

有 许多 不 同 的 方法 可 以 控制 关节 。 后 文 将 区 分 不 精确 控制 器 和 精确 控制 器 : 不 精确 关 
节 控 制 器 将 不 能 在 每 个 可 能 的 调节 步骤 中 提供 新 的 控制 值 (例如 ,一 些 调节 步骤 可 以 /将 跳 
过 ,但 控制 作用 仍然 是 可 以 发 挥 的 )。 另 一 方面 .精确 的 关节 控制 器 将 能 够 在 每 个 可 能 的 调 
节 步 又 中 提供 控制 值 。 

首先 ,用 于 控制 关节 的 方法 将 取决 于 关节 模式 : 

。 关节 不 处 于 力 /力矩 模式 。 

。 关节 以 力 / 力 矩 模 式 操作 。 

两 种 模式 的 不 同 之 处 在 于 力 /力矩 模式 下 操作 关节 采用 的 是 物理 引擎 。 物 理 引擎 将 默 
认 执 行 比 仿真 循环 多 10 倍 的 计算 步 又 : 仿真 循环 以 20Hz( 在 仿真 时 间 ) 运 行 , 而 物理 引擎 
以 200Hz( 也 在 仿真 时 间 ) 运 行 。 如 果 需 要 ,完全 可 以 配置 该 默认 行为 。 
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如 果 关 节 不 是 力 /力矩 模式 , 则 可 以 通过 simSetJointPosition( 或 类 似 地 ,例如 用 于 远程 
API 的 simxSetJointPosition) API 函数 直接 (和 瞬时 地 ) 设 置 其 位 置 。 我 们 可 以 从 子 脚本 、 
TüfF.ROS 节点 或 远程 API 客户 端 执行 此 操作 。 如 果 从 子 脚本 执行 此 操作 , 则 应 在 非 线程 
子 脚 本 的 驱动 部 分 或 在 主 脚本 的 感 测 阶段 (默认 ) 之 前 执行 的 线程 子 脚本 中 完成 。 然 而 ,在 
后 一 种 情况 下 ,请 确保 线程 子 脚本 与 仿真 循环 同步 以 进行 精确 控制 。 

在 下 面 的 线程 子 脚本 示例 中 ,关节 在 位 置 被 控制 loosley, 并 且 没 有 与 仿真 循环 的 同步 : 


—— Following script should run threaded: 
jointHandle = simGetObjectHandle('Revolute joint') 


simSetJointPosition(jointHandle,90 + math.pi/180) —- set the position to 90 degrees 


simWait(2) —— wait 2 seconds (in simulation time) 
simSetJointPosition(jointHandle,180 * math.pi/180) —— set the position to 180 degrees 
simWait(1) —- wait 1 second (in simulation time) 

simSetJointPosition(jointHandle,0 * math.pi/180) —- set the position to 0 degrees 
etc. 


在 下 面 的 线程 子 脚本 示例 中 ,在 每 个 仿真 步骤 中 精确 地 控制 关节 的 位 置 , 即 线程 与 仿真 
循环 同步 : 


—- Following script should run threaded: 


simSetThreadSwitchTiming(200) -- Automatic thread switching to a large value (200ms) 
jointHandle = simGetObjectHandle('Revolute joint') 


simSetJointPosition(jointHandle,90 x math.pi/180) —- set the position to 90 degrees 
simSwitchThread() —- the thread resumes in next simulation step (i.e. when t becomes t * dt) 
sinSetJointPosition(jointHandle,180 x math.pi/180) —- set the position to 180 degrees 
simSwitchThread() —- the thread resumes in next simulation step 
sinSetJointPosition(jointHandle,0 * math.pi/180) -- set the position to 0 degrees 
simSwitchThread() —- the thread resumes in next simulation step 

== ete: 


—- In above code,a new joint position is applied in each simulation step 


当 我 们 尝试 从 外 部 应 用 程序 (例如 ,通过 远程 API 或 ROS) 控 制 非 强制 /力矩 模式 的 关 
节 时 ,外 部 控制 器 将 异步 地 运行 到 V-REP( 即 类 似 于 线程 子 脚本 的 非 同步 代码 )。 这 大 多 用 
于 控制 ,但 是 如 果 希 望 在 每 个 仿真 循环 中 精确 地 控制 关节 的 位 置 , 则 必须 在 同步 模式 下 运行 
V-REP, 并 且 外 部 控制 器 (例如 远程 API 客户 端 ) 必 须 明 确 地 触发 每 个 仿真 步骤 。 下 面 说 明 
了 一 个 C/C++ 远程 API 客户 端 : 
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simxSynchronous (clientId, 1); -- enable the synchronous mode (client side). The server side 

(i.e. V- REP) also needs to be enabled. 

simxStartSimulation(clientId,simx opmode oneshot); //start the simulation 

simxSetJointPosition(clientId, jointHandle, 90.0 * 3. 1415£/180.0f,simx opmode oneshot); 
//set the joint to 90 degrees 

simxSynchronousTrigger(clientId); //trigger next simulation step. Above commands will be applied 

simxSetJointPosition(clientId, jointHandle, , 180. 0 * 3. 1415£/180. 0£, simx opmode oneshot); 
//set the joint to 180 degrees 

simxSynchronousTrigger(clientId);//next simulation step executes. Above commands will be applied 

simxSetJointPosition(clientlId, jointHandle, , 0.0 * 3. 1415£/180. 0f, simx opmode oneshot); 
//set the joint to 0 degrees 

etc. 


如 果 关 节 处 于 力 /力矩 模式 : 如 果 关 节 在 力 /力矩 模式 下 操作 并 且 动 态 地 启用 , 则 其 将 
由 物理 引擎 间接 处 理 。 如 果 关 节 的 电机 没有 启用 ,那么 关节 不 受 控制 ( 即 它 是 可 以 在 外 力作 
用 下 自由 运动 的 ) 。 和 否则 ,关节 可 以 是 以 下 两 种 动态 模式 : 

CD 关节 的 电机 启用 ,但 控制 回路 被 禁用 。 当 我 们 想 要 从 外 部 应 用 程序 (例如 力 /力矩 
控制 ,PID 等 ) 精 确 控制 关节 时 ,使 用 此 模式 。 当 想 要 在 力 /力矩 模式 下 或 用 于 速度 控制 ( 例 
如 机 器 人 轮 马 达 ) 时 不 精确 地 控制 关节 时 ,也 可 使 用 此 模式 。 

(2) 关节 的 电机 启用 ,控制 回路 启用 。 当 关节 需要 用 作 弹 簧 /阻尼 器 ,或 者 想 要 从 
V-REP 中 精确 地 自 定义 控制 关节 时 ,或 者 如 果 想 从 外 部 应 用 程序 不 精确 地 控制 位 置 控制 中 
的 关节 时 ,请 使 用 此 模式 。 

如 果 关 节 的 电机 启用 ,但 控制 回路 被 禁用 , 则 物理 引擎 将 应 用 指定 的 最 大 力 / 转 矩 ,并 加 
速 关节 直到 达到 目标 速度 。 如 果 负 载 小 或 力 和 转 矩 达到 最 大 , 则 将 快速 达到 上 日 标 速度 。 否 
则 ,将 需要 一 些 时 间 。 如 果 力 /力矩 不 够 大 , 则 将 永远 不 会 达到 目标 速度 。 我 们 可 以 使 用 
simSetJointTargetVelocity( 或 例如 ,在 远程 API,simxSetJointTargetVelocity 的 情况 下 ) 以 
及 使 用 simSetJointForce( 或 例如 ,在 远程 API 的 情况 下 ,simxSetJointForce) 的 最 大 力 / 力 
矩 来 以 编程 方式 调整 目标 速度 。 如 果 想 在 脚本 中 编写 力 /力矩 模式 下 的 精确 关节 控制 器 时 ， 
应 该 注意 以 下 问题 ， 

默认 情况 下 ,仿真 循环 以 50ms 的 时 间 步 长 (在 仿真 时 间 内 ) 运 行 。 但 是 物理 引擎 将 以 
5ms 的 时 间 步 长 运行 。 将 在 每 个 仿真 步骤 中 调用 子 脚本 ,但 不 在 每 个 物理 引擎 计算 步 又 中 
调用 。 子 脚本 不 是 在 每 个 物理 引擎 计算 步 又 中 被 调用 ,而 是 在 每 个 仿真 步骤 中 被 调用 。 这 
意味 着 如 果 你 通过 一 个 子 脚本 控制 一 个 关节 , 则 只 能 每 10 个 物理 引擎 计算 步骤 下 提供 一 个 
新 的 控制 值 : 你 将 缺少 9 个 步骤 的 控制 量 计算 与 输出 。 克 服 这 一 点 的 一 种 方法 是 改变 默认 
仿真 设置 ,并 指定 仿真 时 间 步 长 为 5ms, 而 不 是 50ms。 这 是 个 很 好 的 方法 ,但 要 记 住 ,所 有 
其 他 计算 (例如 视觉 传感器 ,接近 传感器 .距离 计算 .IK 等 ) 也 将 运行 10 次 ,最 后 减 慢 你 的 仿 
真 进度 (大 多 数 时 候 你 不 需要 让 其 他 计算 模块 具有 这 样 的 高 刷新 率 ,但 是 物理 引擎 需要 这 样 
高 的 刷新 率 ) 。 另 一 个 更 好 的 选择 是 ,也 需要 启用 关节 的 控制 循环 ,并 在 关节 控制 回调 脚本 
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中 实现 对 它 的 控制 ,这 将 在 后 文 进一步 解释 。 

另 一 方面 ,如 果 想 要 在 外 部 (例如 从 远程 API 客户 端 或 ROS 节点 ) 运 行 精确 和 规则 的 
关节 控制 器 ,那么 除了 将 仿真 循环 设置 为 与 物理 引擎 相同 的 速率 之 外 ,没有 别 的 选择 ,然后 
在 同步 模式 下 运行 V-REP, 外 部 控制 器 (例如 远程 API 客户 端 ) 必 须 显 式 地 触发 每 个 仿真 步 
又 。 下 面 说 明了 这 样 做 的 C/C++ 远程 API 客户 端 : 


simxSynchronous(clientId,1); -- enable the synchronous mode (client side). The server side 
(i.e. V- REP) also needs to be enabled. 

simxStartSimulation(clientId,simx opmode oneshot);//start the simulation 
sinxSetJointForce(clientlId, jointHandle, 1.0f,simx opmode oneshot); 

//set the joint force/torque 
simxSetJointTargetVelocity(clientId, jointHandle, 180. 0f « 3. 1415f/180. 0f, simx  opmode _ 
oneshot); //set the joint target velocity 
simxSynchronousTrigger(clientId); //trigger next simulation step. Above commands will be applied 
sinxSetJointForce(clientId, jointHandle, 0.5f, simx opmode oneshot); 

//set the joint force/torque 
simxSetJointTargetVelocity(clientId, jointHandle, 180. 0f * 3. 1415f/180. Of, simx  opmode _ 
oneshot) ; //set the joint target velocity 
simxSynchronousTrigger(clientlId); //next simulation step executes. Above commands will be applied 
simxSetJointForce(clientId, jointHandle, 2.0f,simx opmode oneshot); 

//set the joint force/torque 
simxSetJointTargetVelocity(clientId, jointHandle, 180. 0f * 3. 1415f/180. Of, simx  opmode _ 
oneshot) ; //set the joint target velocity 
etc. 


如 果 关 节 的 电机 启用 ,并 且 控 制 回路 也 启用 ,物理 引擎 将 根据 设置 控制 关节 : 可 以 对 关 
节 进 行 位 置 控制 ( 即 PID 控制 ) ,或 者 在 弹簧 /阻尼 器 模式 和 自 定义 控制 模式 下 进行 控制 。 
PID 和 弹簧 /阻尼 器 参数 可 以 从 子 脚本 .远程 API 客户 端 或 ROS 节点 更 新 。 与 对 象 参数 ID 
2002-2004 和 2018-2019 有 关 。 所 需 的 目标 位 置 可 以 使 用 simSetJointTargetPosition( 例 如 ， 
从 远程 API 客户 端 ,simxSetJointTargetPosition) 设 置 。 当 需要 一 个 精确 的 自 定义 控制 器 ， 
应 该 使 用 关节 控制 回调 脚本 ,而 不 是 从 一 个 子 脚本 中 控制 关节 。 

最 后 ,如 果 需 要 在 外 部 应 用 程序 中 实现 精确 的 PID 或 自 定义 的 控制 器 , 则 需要 确保 仿 
真 步 长 与 物理 引擎 计算 步 长 相同 : 默认 情况 下 ,V-REP 的 仿真 环 路 运行 在 20Hz 在 仿真 时 
Ta] ,而 物理 引擎 运行 在 200Hz。 我 们 可 以 在 仿真 设置 中 调整 仿真 步 长 。 还 需要 确保 的 是 : 
在 同步 模式 下 运行 的 V-REP。 下 面 说 明了 一 个 C/C++ 远程 API 客户 端 : 

simxSynchronous (clientId,1); -- enable the synchronous mode (client side). The server side 


(i.e. V- REP) also needs to be enabled. 
simxStartSimulation(clientId,simx opmode oneshot); //start the simulation 
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simxSetJointTargetPosition ( client1d, jointHandle, 90. Of + 3. 1415f/180. 0f, simx opmode | 
oneshot); //set the desired joint position 

simxSynchronousTrigger(clientId); //trigger next simulation step. Above commands will be applied 
sinxSetJointTargetPosition(clientld, jointHandle, 180. 0f * 3. 1415f/180. 0f, simx _ opmode _ 


oneshot); //set the desired joint position 

simxSynchronousTrigger (clientId); //next simulation step executes. Above commands will 
be applied 

simxSetJointTargetPosition(clientId, jointHandle, 0. Of « 3. 1415f/180. 0f, simx  opmode _ 
oneshot); //set the desired joint position 

etc. 


也 可 以 通过 远程 API 客户 端 来 为 关节 控制 器 提供 控制 量 ,这 将 需要 给 关节 控制 回调 脚 
本 提供 信号 实现 ,如 下 例 所 示 : 


simxSynchronous(clientId,1); -- enable the synchronous mode (client side). The server side 
(i.e. V- REP) also needs to be enabled. 
simxStartSimulation(clientId,simx opmode oneshot); //start the simulation 
simxSetFloatSignal(clientId, "myDesiredTorque",1.0f,simx opmode oneshot); 
//set the signal value 
simxSetFloatSignal(clientId, "myDesiredTarget",90.0f * 3.1415/180. 0f, simx opmode oneshot); 
//set the signal value 
simxSynchronousTrigger(clientId); //trigger next simulation step. Above commands will be applied 
etc. 


在 上 面 的 例子 中 ,关节 控制 回调 脚本 可 以 在 进行 控制 之 前 获取 这 两 个 信号 (使 用 
simGetFloatSignal) 。 

4. 关节 的 性 质 

关节 属性 是 场景 对 象 属性 对 话 框 的 一 部 分 ,可 以 通过 依次 单 击 菜单 栏 .工具 ,场景 对 象 
属性 打开 。 还 可 以 通过 双击 场景 层次 结构 中 的 对 象 图 标 或 单 击 其 工具 栏 按钮 来 打开 对 
话 框 。 

1) 关节 的 普通 性 质 

在 场景 对 象 属性 对 话 框 中 , 单 击 关 节 按 钮 显示 关节 对 话 框 (如 果 最 后 一 个 选择 是 关节 ， 
则 仅 显 示 关 节 按 钮 )。 如 图 3-43 所 示 , 该 对 话 框 显示 最 后 选择 的 关节 的 设置 和 参数 。 如 果 
选择 了 多 个 关节 , 则 可 以 将 一 些 参数 从 最 后 选择 的 关节 复制 到 其 他 所 选 关 节 。 请 注意 ,这 仅 
在 相同 类 型 或 模式 的 关节 之 间 具 有 效果 。 

位 置 是 循环 的 : 指示 关节 位 置 是 否 是 循环 的 (在 一 180 和 十 180 之 间 变 化 ,没有 限制 ) 。 
只 有 旋转 关节 可 以 是 循环 的 。 

螺 距 : 关节 的 螺 距 值 。 此 属性 仅 适用 于 Revolute/Screw 类 型 的 关节 ,并 且 仅 在 “位 置 
是 循环 的 ”选项 框 中 保留 未 选中 的 情况 下 该 属性 才 有 效 。 
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Scene Object Properties B 
[ Jont ] Common. ] 
Configuraton 
C Poston is cyd Saew pitch [m/deg] +0.00 +00 


Pos. mn. [deg] [-1200e4+02 | Pos. range [deg] 。 [2400e402 | 
Position [deg] ^ +0.000e+00 | 
IKcaodatonweght [100 
Max. step size [deg]  [100e401 


图 3-43 关节 对 话 框 


位 置 最 小 值 : 非 循环 旋转 关节 ,螺钉 或 棱柱 关节 的 最 小 允许 值 。 

位 置 范围 : 非 循环 旋转 关节 ,螺杆 或 棱柱 关节 的 变化 范围 。 这 样 的 关节 的 位 置 被 限制 
在 一 定 的 位 置 范围 内 。 

位 置 : 旋转 关节 ,棱柱 关节 或 螺钉 的 内 在 关节 位 置 。 

IK 计算 重量 : 逆 运动 学 计算 期 间 关 节 的 重量 。 例 如 ,在 元 余 机 械 手 的 情况 下 ,该 选项 
使 我 们 能 够 在 反 向 运动 学 决议 期 间 让 某 些 关节 超过 其 他 关节 。 与 其 他 关节 相 比 ,具有 较 小 
重量 的 关节 将 具有 相对 较 小 的 位 置 变化 。 

最 大 步 长 : 在 一 个 运动 学 计算 期 间 允 许 的 最 大 位 置 变化 。 较 小 的 步 长 通常 导致 较 大 的 
计算 量 , 但 可 以 更 稳定 。 对 于 道 运 动 学 计算 ,该 值 可 以 被 忽略 最 大 值 覆盖 。 步 长 项 目 在 逆 运 
动 学 的 对 话 框 中 。 

模式 : 关节 的 控制 模式 。 关 节 可 以 处 于 被 动 模式 、 反 向 运动 学 模式 ,依赖 模式 或 力矩 / 
力 模 式 。 

混合 操作 : 当 关 节 处 于 被 动 模式 、 反 向 运动 学 模式 或 依赖 模式 时 ,也 可 以 选择 以 混合 方 
式 操 作 。 混 合 操作 允许 关节 以 常规 方式 操作 ,但 是 ,在 动态 计算 之 前 ,当前 关节 位 置 将 复制 
到 目标 关节 位 置 ,然后 在 动力 学 计算 期 间 ,关节 的 控制 将 处 理 成 电机 的 位 置 控制 ( 当 且 仅 当 
其 被 动态 启用 时 ) 。 

调整 依赖 关系 方程 : 如 果 关 节 处 于 依赖 模式 , 则 可 以 指定 将 关节 连 杆 到 另 一 关节 的 线 
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性 方程 。 对 话 框 中 ,此 部 分 的 值 均 以 米 或 弧度 表示 。 

长 度 : 关节 的 长 度 。 没 有 功能 意义 。 

直径 : 关节 的 直径 。 没 有 功能 的 意义 。 

调整 颜色 A/B: 颜色 A 是 关节 固定 部 分 的 颜色 ,颜色 B 是 关节 活动 部 分 的 颜色 。 

显示 动态 属性 对 话 框 : 切换 关节 动力 学 属性 对 话 框 。 关 节 动 态 对 话 框 允许 调整 关节 的 
动态 属性 。 

2) 关节 的 动力 学 性 质 

关节 动力 学 性 质 是 关节 性 质 的 一 部 分 。 如 图 3-44 所 示 ,关节 动力 学 对 话 框 显示 了 关节 
的 动力 学 设置 和 参数 。 如 果 未 选择 对 象 , 则 对 话 框 处 于 非 活动 状态 。 如 果 选 择 了 多 个 关节 ， 
则 可 以 将 最 后 选择 的 关节 的 一 些 参 数 复制 到 其 他 所 选 关 节 。 请 注意 ,这 仅 在 相同 类 型 或 模 
式 的 关节 之 间 具 有 效果 。 


Joint Dynamic Properties 


Motor properties 

[| Motor enabled 

Target velocity [deg/s] 

Maximum torque [N*m] 1.76006 402 
Lock motor when target velooty s zero. 


图 3-44 关节 动力 学 对 话 框 


电机 启用 : 启用 或 禁用 关节 的 电机 。 如 果 禁 用 ,关节 是 可 以 自由 运动 的 。 仅 当 关节 处 
于 力矩 / 力 模式 时 可 用 。 

目标 速度 : 关节 电机 的 目标 速度 。 如 果 最 大 力矩 / 力 足 够 高 ,目标 速度 将 立即 达到 ;， 和 否 
则 逐渐 逼近 目标 速度 。 

最 大 力矩 / 力 : 关节 电机 运行 时 的 最 大 力矩 或 力 。 

当 目 标 速度 为 零 时 锁定 电机 : 当 关 节 的 电机 启用 并 且 控 制 回 路 被 禁用 时 , 它 在 速度 控 
制 中 起 作用 。 当 目标 速度 为 零 时 .其 可 能 漂移 .因为 力矩 / 力 将 仅 作为 内 部 摩擦 。 为 了 避免 
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这 种 情况 ,可 以 启用 此 项 目 , 当 目标 速度 设置 为 零 时 ,将 锁定 关节 。 

引擎 特定 属性 : 允许 调整 引擎 特定 参数 。 

控制 回路 已 启用 : 启用 或 禁用 关节 控制 回路 。 默 认 情 况 下 ,使 用 内 置 PID 控制 器 。 

目标 位 置 : 期 望 的 目标 位 置 。 

速度 上 限 : 允许 将 速度 限制 在 最 大 值 。 

自 定义 控件 : 如 果 启 用 , 则 将 通过 关节 控件 回调 脚本 控制 关节 。 这 允许 用 户 写 入 非常 
特定 的 关节 控制 器 ,其 将 在 每 个 物理 引擎 仿真 步 长 ( 即 , 默 认为 仿真 时 间 步 长 的 10 fO 下 
执行 。 

编辑 自 定义 控制 循环 : 允许 编辑 关节 控制 回调 脚本 来 获得 关节 的 自 定义 控制 。 

位 置 控制 (PID): 如 果 启 用 , 则 通过 内 置 PID 控制 器 调节 关节 速度 ,从 而 控制 关节 的 
位 置 。 

比例 /积分 /微分 参数 : PID 位 置 控 制 参数 。 

弹簧 减 震 器 模式 : 如 果 启 用 , 则 通过 内 置 弹簧 减 震 器 控制 器 调节 关节 力 /力矩 ,从 而 控 
制 关 节 的 位 置 。 

弹簧 常数 K /阻尼 系数 C: 弹簧 阻尼 器 参数 。 

3) 动力 学 引擎 与 关节 相关 的 属性 

与 关节 相关 的 动力 学 引擎 属性 可 以 通过 关节 动力 学 属性 对 话 框 访问 。 最 后 选择 的 关节 
的 引擎 特定 属性 显示 在 对 话 框 中 。 如 果 选 择 了 多 个 关节 , 则 可 以 将 最 后 选择 的 关节 的 属性 
复制 到 其 他 所 选 关节 (将 所 有 属性 应 用 于 所 选 关 节 ) 。 

(1) 项 目 符号 属性 : 与 Bullet 物理 库 相 关 的 属性 。 

正常 CFM: 当 远 离 极 限时 使 用 的 约束 力 混 合 参数 。 

停止 ERP: 在 极限 处 的 误差 减 小 参数 。 

停止 CFM: 在 极限 处 约束 力 混合 参数 。 

(2) ODE 属性 : 与 Open Dynamics Engine 相关 的 属性 。 

正常 CFM: 当 远离 极限 时 使 用 的 约束 力 混 合 参数 。 

停止 ERP: 在 极限 处 的 误差 减 小 参数 。 

停止 CFM: 在 极限 处 的 约束 力 混 合 参数 。Bounce 是 指 调节 在 限制 下 的 弹跳 性 的 值 ( 即 
恢复 参数 ) 。 

Fudge AF: 一 个 任意 值 ,可 以 使 关节 正确 地 运行 。 其 值 在 远离 极限 时 不 太 跳跃 。 

(3) 涡流 性 质 : 与 Vortex Dynamics 引擎 相关 的 属性 。 

关节 轴 摩 擦 : 允许 沿 约束 轴线 或 围绕 约束 轴线 定义 内 部 摩擦 。 

启用 : 启用 内 部 摩擦 。 

比例 : 如 果 为 真 ,摩擦 力 与 约束 中 的 张力 成 正比 。 

系数 : 对 于 比例 摩擦 力 ,摩擦 力 与 张力 系数 成 正比 。 

最 大 力 : 对 于 非 比例 摩擦 ,摩擦 力 可 以 由 用 户 直 接 提供 。 

损耗 : 粘度 系数 。 
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关节 轴 限 制 : 管理 沿 着 或 围绕 约束 轴 的 位 置 或 角度 限制 。 
下 /上 恢复 : 弹性 。 
下 /上 刚度 : 限制 的 弹簧 的 线性 角 刚 度 。 
下 /上 阻尼 : 限制 的 弹簧 的 线性 角 阻 尼 。 
下 /上 最 大 力 : 限制 弹簧 的 力 或 力矩 边界 值 。 
关节 依赖 性 : 属性 允许 动态 连 杆 两 个 启用 了 动态 连 杆 功能 的 关节 。 两 个 连接 的 关节 将 
共享 /交换 它们 各 自 的 负载 ( 即 ,施加 到 一 个 关节 中 的 力 /力矩 将 被 传递 到 另 一 个 关节 中 , 反 
之 亦 然 ) 。 
从 属 关节 : 与 此 关节 关联 的 关节 。 此 属性 不 是 双向 显示 的 ( 即 , 依 赖 关系 将 不 在 从 属 关 
WEER) ,以便 能 够 将 依赖 关节 连 杆 到 其 他 关节 ,从 而 建立 复杂 的 依赖 关系 。 所 有 从 属 关 
节 必 须 共 享 相同 的 父 形状 ,这 样 才能 获得 正确 的 操作 。 
乘法 因子 : 连 杆 依赖 关节 的 乘法 因子 。 一 般 的 或 为 1 的 因子 将 使 两 个 连 杆 的 旋转 关节 
以 相同 的 速率 在 相同 的 方向 上 旋转 。 
偏 移 : 尚未 使 用 。 保 持 在 0。 
X 轴 位 置 : 沿 约束 X 轴 ( 位 置 ) 的 松弛 和 摩擦 参数 。 
松弛 : 松弛 人 允许 控制 相应 的 约束 强度 。 
。 启用 : 启用 松弛 属性 。 
* 刚度: 基于 位 置 的 方程 的 弹性 刚度 。 
* 阻尼: 基于 位 置 的 方程 的 弹 答 阻 尼 。 
* 损失; 基于 速度 的 方程 的 黏度 。 
摩擦 : 沿 轴 的 内 摩擦 。 
启用 : 启用 此 轴 的 内 部 摩擦 。 
比例 : 使 摩擦 与 关节 中 的 张力 成 比例 。 
系数 : 摩擦 设 定 为 张力 系数 。 
* RAH: 如 果 不 是 成 比例 的 摩擦 ,摩擦 力 由 用 户 手动 设置 。 
。 损耗 : 摩擦 力 黏 度 。 
Y 轴 位 置 : 沿 约 束 Y 轴 ( 位 置 ) 的 松弛 和 摩擦 参数 。 
Z 轴 位 置 : 沿 约束 Z 轴 ( 位 置 ) 的 松弛 和 摩擦 参数 。 
X 轴 取向 : 围绕 约束 X 轴 (定向 ) 的 松弛 和 摩擦 参数 。 
Y 轴 取 向 : 围绕 约束 Y 轴 ( 取 向 ) 的 松弛 和 摩擦 参数 。 
Z 轴 取向 : 围绕 约束 Z 轴 ( 定 向 ) 的 松弛 和 摩擦 参数 。 
(4) Newton 属性 : 与 Newton 动力 学 引擎 相关 的 属性 。 
关节 依赖 性 : 属性 允许 动态 连 杆 两 个 动态 启用 的 关节 ,两 个 连接 的 关节 将 共享 /交换 它 
们 各 自 的 负载 ( 即 ,施加 到 一 个 关节 中 的 力 /力矩 将 被 传递 到 另 一 个 关节 中 ,反之 亦 然 ) 。 
从 属 关节 : 与 此 关节 关联 的 关节 。 此 属性 不 是 双向 显示 的 ( 即 , 依 赖 关 系 将 在 从 属 关节 
上 显示 ) ,以 便 能 够 将 依赖 关节 连 杆 到 其 他 关节 ,从 而 建立 复杂 的 依赖 关系 。 所 有 从 属 关节 
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必须 共享 相同 的 父 形状 ,这 样 才能 获得 正确 的 操作 。 

乘法 因子 : 连 杆 依赖 关节 的 乘法 因子 。 一 般 的 或 为 1 的 因子 将 使 两 个 连 杆 的 旋转 关节 
以 相同 的 速率 在 相同 的 方向 上 旋转 。 

偏 移 : 尚未 使 用 。 保 持 为 0。 


3.4.5 V-REP 的 集合 


集合 是 用 户 定 义 的 场景 对 象 集合 。 集 合 必须 包含 至 少 一 个 对 象 ,并 被 认为 是 一 个 实体 
(对 象 也 是 实体 )。 集 合 在 引用 几 个 对 象 (例如 机 器 人 ) 时 非常 有 用 。V-REP 不 仅 支持 对 象 ， 
还 支持 基于 集合 的 计算 。 例 如 ,碰撞 检测 模块 允许 注册 以 下 碰撞 对 : (集合 A; 对 象 B)。 然 
后 ,冲突 检查 算法 将 检查 集合 A( 构 成 它 的 任何 对 象 ) 是 否 与 对 象 B 冲突 。 

集合 是 可 碰撞 、 可 测量 、 可 检测 、 可 切割 和 可 呈现 的 实体 。 这 意味 着 集合 具有 以 下 的 
属性 : 

CD 集合 可 用 于 对 其 他 可 碰撞 实体 的 磁 撞 检测 。 

(2) 集合 可 用 于 与 其 他 可 测量 的 实体 的 最 小 距离 的 计算 。 

(3) 集合 可 以 由 接近 传感器 检测 。 

(4) 集合 可 以 由 磨 机 切割 。 

(5) 集合 可 以 被 视觉 传感器 检测 或 看 到 。 

(6) 即使 集合 是 可 碰撞 、 可 测量 .可 检测 .可 切割 和 可 泻 染 的 ,这 并 不 意味 着 集合 中 包含 
的 所 有 对 象 都 是 可 碰撞 、 可 测量 .可 检测 .可 切割 或 可 泻 染 的 。 
在 碰撞 检测 期 间 , 仅 对 集合 (集合 的 子 集 ) 的 可 碰撞 对 象 与 男 一 可 碰撞 实体 进行 
检测 。 
在 距离 测量 期 间 , 仅 对 在 集合 中 的 距离 可 测量 的 对 象 ( 集 合 的 子 集 ) 进 行 与 男 一 个 可 
测量 实体 之 间 的 距离 测量 。 
只 有 集合 中 可 检测 的 对 象 (集合 的 子 集 ) 可 以 由 接近 传感器 检测 。 

。 只 有 集合 中 可 切割 对 象 (集合 的 子 集 ) 可 以 被 铣 刀 切割 。 

。 只 有 集合 的 可 泻 染 对 象 (集合 的 子 集 ) 可 以 被 视觉 传感器 检测 到 。 

然而 ,集合 可 以 覆盖 其 对 象 的 属性 ,这 些 属 性 包括 可 磁 撞 的 、 可 测量 的 .可 检测 的 、 可 剪 
切 的 和 可 呈现 的 。 集 合 必须 由 至 少 一 个 元 素 组 成 。 支 持 以 下 元 素 : 所 有 场景 对 象 ; 松散 物 
fk; 算 上 基 的 树 ; 不 算 基 的 树 ; 包含 末端 的 链 ; 不 包含 末端 的 链 ; 以 及 上 述 元 素 的 组 合 。 

1. 所 有 场景 对 象 

元 素 由 所 有 场景 对 象 组 成 ,如 图 3-45 所 示 (箭头 表示 : 子 元 素 )。 

上 面 的 元 素 没有 任何 定义 对 象 ,不 能 单独 存在 ,需要 结合 其 他 类 型 的 元 素 。 

2. 松散 物体 

这 是 一 个 可 松散 定义 的 场景 对 象 的 组 合 ,如 图 3-46 所 示 ( 箭 头 表 示 : 子 元 素 )。 

在 上 面 的 示例 中 ,如 果 从 场景 中 删除 对 象 2、 对 象 3、 对 象 4 和 对 象 7, 则 该 元 素 不 再 有 
效 , 并 且 也 将 被 删除 。 对 象 2、 对 象 3、 对 象 4 和 对 象 7 是 元 素 的 定义 对 象 。 
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有 的 对 次 元 素 (松散 的 对 象 ) 
| 00 dj 
| 1 bd | | L-----d----- 
1 [ag 对 象 3 | 1 [对 象 2 对 象 3 | 
m oj MAA C 
| "TE aj i | 
| 09) 四 | 四 
图 3-45 由 所 有 场景 对 象 组 成 的 元 素 图 3-46 由 松散 物体 组 成 的 元 素 
3. 树 (包括 基 ) 
这 包括 一 个 对 象 及 其 所 有 子 对 象 (和 子 对 象 的 子 对 象 等 ), 如 图 3-47 Bros (箭头 表 示 : 
子 对 象 )。 
在 上 面 的 示例 中 ,如 果 从 场景 中 删除 对 象 1, 则 元 素 不 再 有 效 , 并 且 将 被 删除 。 对 象 1 
是 元 素 的 定义 对 象 。 
4. 树 (不 包括 基 ) 


这 包括 对 象 的 所 有 子 对 象 ( 和 子 对 象 的 子 对 象 等 ), 不 包括 对 象 本 身 .如 图 3-48 所 示 ( 箭 
头 表示 : 子 对 象 )。 


A 


元 素 ( 树 ， 不 包括 树 的 基 ) 


"d N 
[i 1 
1 ! 对 象 4 对 象 5 对 象 6 | 
! 1 jw i 
1 1 
| | 


M 
DIS d oy mr UR A A cu is ac de adi dias 一 
Ane 


图 3-47. 由 一 棵 树 ,包括 基地 组 成 的 元 素 图 3-48 由 一 棵 树 组 成 的 元 素 , 不 包括 基 


在 上 面 的 示例 中 ,如 果 从 场景 中 删除 对 象 1, 则 元 素 不 再 有 效 ,并 且 将 被 删除 。 对 象 1 


是 元 素 的 定义 对 象 。 

5. 链 ( 包 括 末 端 ) 

它 包括 一 个 对 象 和 它 的 所 有 父 对 象 ( 和 父 对 象 的 父 对 象 等 ), 如 图 3-49 所 示 ( 箭 头 表示 : 
子 对 象 )。 


在 上 面 的 示例 中 ,如 果 从 场景 中 删除 对 象 6, 则 元 素 不 再 有 效 ,并 且 将 被 删除 。 对 象 6 
是 元 素 的 定义 对 象 。 
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6. 链 ( 末 端 除外 ) 
这 包括 对 象 的 所 有 父 对象 ( 和 父 对 象 的 父 对 象 等 ) ,不 包括 对 象 本 身 , 如 图 3-50 ros Cf 
头 表 示 : 是 子 对 象 ) 。 


元 素 (包括 链 和 末端) Es 
MP 元 素 (不 包括 链 ， 末 端 ) 


加 四 到 (res) (es) (aeo) 


m S 


1 
IJ 
i 
1 
1 1 ! [ 
CUICON e] [ve] | 
1 1 
IJ L| 1 
I| 对 象 7 1 |y 中 对 象 7 
E STIRE wn 


一 
- 

1 

1 
一 -| 

1 

[ 

1 
zi 


! [ue 
| EI E 
图 3-49. 由 链条 组 成 的 元 件 , 包 括 尖 端 图 3-50 由 链条 组 成 的 元 件 , 不 包括 尖端 


在 上 面 的 示例 中 ,如 果 从 场景 中 删除 对 象 6, 则 元 素 不 再 有 效 , 也 将 被 删除 。 对 象 6 是 
元 素 的 定义 对 象 。 
7. 元 素 的 组 合 
在 集合 中 ,元素 可 以 是 加 法 或 减法 。 如 图 3-51 所 示 ,显示 了 由 3 个 元 素 定义 的 集合 。 
LA (场景 中 的 所 有 对 象 ) 


: 


元 素 2 : WEERA 诚 法 (包括 连 杆 和 未 端 ) 
(a) 三 个 元 素 (b) 由 三 个 元 素 组 成 的 所 得 集 人 台 


图 3-51 由 三 个 元 素 定义 的 集合 


在 上 面 的 示例 中 ,生成 的 集合 包含 两 个 对 象 。 如 果 新 对 象 附加 到 对 象 2 或 对 象 4, 则 它 
不 会 包含 在 集合 中 (因为 它 将 是 元 素 2 中 定义 的 树 的 一 部 分 , 它 是 减法 的 ) 。 如 果 新 对 象 成 
为 对 象 5、 对 象 3 或 对 象 1 的 父 对 象 , 则 它 不 会 包含 在 集合 中 (因为 它 将 是 元 素 3 中 定义 的 
链 中 的 一 部 分 ,这 是 减法 的 ) 。 否 则 ,新 对 象 将 自动 包含 在 集合 中 。 集 合 是 动态 实体 ,会 自动 
重新 计算 或 更 新 。 如 果 一 个 对 象 为 定义 对 象 , 当 对 它 进 行 复制 或 保存 时 ,相关 集合 也 将 被 自 
动 地 复制 或 保存 。 

不 用 盲目 地 使 用 集合 ,只 有 在 没有 找到 其 他 替代 品 的 时 候 可 以 考虑 使 用 。 例 如 ,如 果 和 希 
望 测试 移动 机 器 人 与 场景 中 所 有 其 他 对 象 的 碰撞 ,可 以 定义 机 器 人 集合 (从 机 器 人 底层 开始 


258 号 | 机 器 人 仿真 与 编程 技术 


建立 的 树 ) 和 障碍 集合 ,然后 对 机 器 人 集合 和 障碍 物 集合 进行 碰撞 检测 。 这 种 做 法 是 没有 问 
题 的 ,但 是 障碍 物 集合 不 是 必需 的 。 事 实 上 ,碰撞 检测 模块 允许 指定 场景 项 目 中 的 所 有 其 他 
可 碰撞 的 对 象 进行 碰撞 检测 。 

下 面 介绍 集合 对 话 框 。 依 次 单 击 菜单 栏 .工具 、 集 合 可 以 打开 集合 对 话 框 。 或 者 ,也 可 
以 通过 其 工具 栏 按钮 进行 访问 。 集 合 的 对 话 框 如 图 3-52 所 示 。 


Composing elements and attributes 
+A objects [-] 
-From object (incl. up [LBR. ilwa 14 R&20] 


[O Colection overrides colidable, measurable, renderable, 
Guttable & detectable properties 


图 3-52 集合 的 对 话 框 


添加 新 集合 : 添加 一 个 新 的 空 集合 。 尽 量 为 集合 提供 唯一 的 名 称 , 以 便 在 与 其 他 模型 
合成 场景 时 不 会 发 生 名 称 冲突 。 集 合 可 以 在 集合 列表 中 重 命名 。 

可 视 化 所 选集 合 : 选择 场景 中 所 有 相关 的 对 象 , 则 这 些 对 象 的 集合 是 可 见 的 。 

对 所 选集 合 的 操作 : 对 话 框 的 此 部 分 允许 向 所 选集 合 添加 或 从 所 选集 合 中 减 去 各 种 对 
象 。 添 加 新 集合 时 , 它 最 初 为 空 。 然 后 可 以 添加 或 减 去 单个 对 象 ( 选 定 对 象 )` 层 级 树 中 包含 
的 所 有 对 象 ( 选 定 对 象 的 树 )、 链 中 包含 的 所 有 对 象 ( 选 定 对 象 的 链 - 即 所 选 对 象 的 所 有 父 级 
和 祖先 ) 或 场景 中 的 所 有 对 象 (所 有 对 象 ) 。 

合 元 素 和 属性 : 组 成 所 选集 合 的 元 素 列 表 。 可 以 使 用 键盘 上 的 删除 键 删除 单个 
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元 素 。 
集合 覆盖 可 碰撞 、 可 衡量 .可 呈现 ` 可 切割 和 可 检测 的 属性 : 允许 覆盖 集合 中 的 对 象 的 
主要 属性 (可 碰撞 、 可 衡量 .可 呈现 .可 剪 切 和 可 检测 的 ) 。 例 如 ,如 果 和 希望 一 些 对 象 是 不 可 检 
测 的 ,除了 特殊 情况 ,可 以 简单 地 禁用 这 些 对 象 的 可 检测 的 属性 (参考 对 象 常见 属性 ), 然 后 
将 这 些 对 象 建立 集合 ,并 启用 可 检测 属性 。 现 在 , 当 且 仅 当 它们 作为 刚刚 定义 的 集合 时 ,这 
些 对 象 才 可 以 被 检测 到 ( 换 句 话说 ,只 有 当 尝 试 检测 它们 的 集合 时 ,它们 才能 被 检测 到 ) 。 


3.5 V-REP 的 六 种 计算 模块 


V-REP 具有 六 种 计算 模块 ,包括 了 碰撞 检测 模块 ,最 小 距离 计算 模块 、 逆 运动 学 计算 模 
块 、 几 何 约束 求解 模块 ,动力 学 模块 和 运动 的 路 径 规划 模块 。 这 些 计算 模块 不 是 封装 在 对 象 
中 ,但 可 以 直接 对 一 个 或 两 个 场景 对 象 进行 操作 。 
一 些 计算 模块 允许 被 使 用 在 用 户 定义 的 计算 对 象 中 。 计 算 对 象 与 场景 对 象 有 一 定 的 差 
别 , 但 它们 的 操作 却 有 一 定 的 联系 。 以 下 有 几 种 计算 对 象 本 身 不 存在 的 情况 : 
。 碰撞 检测 对 象 依靠 可 碰撞 的 场景 对 象 。 
。 最 小 距离 计算 对 象 的 距离 依赖 可 测量 的 对 象 。 
。 道 运动 学 计算 对 象 主要 依靠 虚拟 点 和 运动 链 , 主 要 用 在 场景 对 象 为 关节 的 情况 中 。 
* 几何 约束 求解 对 象 主要 依靠 虚拟 点 和 运动 链 , 主 要 在 场景 对 象 为 关节 中 。 
。 关 于 计算 模块 的 设置 ,可 以 通过 依次 单 击 Menu bar (菜单 栏 )、Tools (工具 )、 
Calculation module properties( 计 算 模块 属性 ) 和 工具 栏 2 的 计算 模块 图 标 打 开 计算 
模块 属性 对 话 框 。 


3.5.1 磁 撞 检测 模块 


碰撞 检测 模块 : 允许 对 任何 形状 或 任何 形状 的 集合 组 合 进行 快速 干涉 检查 ,检测 它们 
之 间 是 否 发 生 了 碰撞 (这 种 碰撞 完全 独立 于 动力 学 模块 关于 碰撞 反应 的 计算 算法 ) 。 该 模块 
使 用 基于 二 又 树 的 数据 结构 ,使 用 了 有 向 包围 盒 (OBB) 的 算法 ,使 得 计算 速度 更 加 快速 。 此 
外 ,采用 了 一 种 时 序 一 致 性 缓存 技术 对 这 个 模块 进行 了 算法 优化 。 

V-REP 可 以 以 非常 灵活 的 方式 检测 两 个 可 冲突 实体 之 间 的 碰撞 。 该 计算 是 一 个 精确 
的 干扰 计算 。 碰 撞 检测 模块 将 仅 检 测 碰撞 ;但 是 它 不 会 直接 对 它们 做 出 反应 (对 于 碰撞 响 
应 ,参考 动态 模块 ) 。 

碰撞 检测 模块 允许 登记 那些 可 冲突 的 实体 对 (碰撞 实体 和 碰撞 实体 ) 的 碰撞 对 象 。 在 仿 
真 期 间 , 每 个 注册 的 碰撞 对 象 的 碰撞 状态 可 以 用 不 同 的 着 色 可 视 化 ,或 记录 在 图 形 对 象 中 。 
如 果 注 册 的 碰撞 对 象 的 两 个 组 成 实体 在 复制 粘贴 操作 中 同时 复制 , 则 注册 的 碰撞 对 象 也 会 
自动 复制 。 

碰撞 检测 对 话 框 是 计算 模块 属性 对 话 框 的 一 部 分 ,可 以 通过 依次 单 击 菜单 栏 . 工 具 、 计 
算 模 块 属性 来 打开 。 碰 撞 检 测 对 话 框 如 图 3-53 所 示 ,对 它 的 说 明 如 下 。 
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Colisondetec. | Distance cak. — Pathplannng Motion plann. 
Inversekinematcs Geometric constraint solver Dynamics 


图 3-53 ”碰撞 检测 对 话 框 


启用 所 有 碰撞 检测 : 允许 启用 或 禁用 所 有 已 注册 碰撞 对 象 的 碰撞 检测 。 

添加 新 的 碰撞 对 象 : 允许 指定 两 个 实体 进行 碰撞 检测 。 按 钮 下 方 的 列表 显示 所 有 已 注 
册 的 碰撞 对 象 , 双 击 它们 可 以 重 命名 。 可 以 选择 列表 中 的 各 个 碰撞 对 象 , 然 后 在 下 面 显 示 相 

调整 碰撞 颜色 : 允许 调整 碰撞 实体 的 颜色 。 人 参见 碰撞 更 改 硕 撞 体 颜色 和 碰撞 更 改 磁 撞 
颜色 项 目 获 取 进 一 步 信息 。 

显 式 处 理 : 指示 是 否 应 显 式 处 理 选 定 的 碰撞 对 象 。 如 果 选 中 , 当 调 用 simHandleCollision 
(sim_handle_all_except_explicit) 时 ,但 仅 当 调用 simHandleCollision(sim_handle_all) 或 
simHandleCollision(collisionObjectHandle) 时 ,不 会 处 理 此 碰撞 对 象 的 碰撞 检测 。 如 果 用 
户 希 望 在 子 脚本 中 而 不 是 在 主 脚本 中 处 理 该 碰撞 对 象 的 碰撞 检测 , 则 这 是 有 用 的 (如 果 未 选 
中 , 则 该 碰撞 对 象 的 碰撞 检测 将 被 处 理 两 次 ,一 次 是 在 主 脚本 中 调用 simHandleCollision 
(sim handle all except explicit) 时 , 另 一 次 则 是 在 子 脚本 中 调用 simHandleCollision 
(collisionObjectHandle) 时 )。 

碰撞 更 改 碰撞 /碰撞 颜色 : 在 碰撞 期 间 启 用 或 禁用 碰撞 /碰撞 实体 的 颜色 更 改 。 

Comp,coll,contour(shapes only) : 如 果 启 用 , 则 将 对 形状 -形状 的 碰撞 执行 完全 碰撞 检 
测 ,计算 并 可 视 化 所 有 的 相交 点 ( 即 碰撞 轮廓 ) 。 然 而 ,这 需要 比 简单 碰撞 检测 多 得 多 的 计算 
时 间 。 

调整 轮廓 颜色 : 允许 调整 碰撞 轮廓 线 的 颜色 。 

轮廓 宽度 : 碰撞 轮廓 线 的 宽度 。 


第 3 章 “V-REP 在 机 器 人 仿真 中 的 应 用 | 261 


3.5.2 最 小 距离 计算 模块 


最 小 距离 计算 模块 允许 快速 地 计算 形状 与 形状 之 间 、 形 状 与 形状 的 组 合 之 间 、 形 状 的 组 
合 与 形状 的 组 合 之 间 的 最 小 距离 。 该 模块 使 用 了 与 磁 撞 检测 模块 相同 的 数据 结构 。 此 外 ， 
该 模块 也 采用 了 时 序 一 致 性 缓存 技术 。 
V-REP 可 以 以 非常 灵活 的 方式 测量 两 个 可 测量 实体 之 间 的 最 小 距离 。 该 计算 是 精确 
的 最 小 距离 计算 。 距 离 计算 模块 将 仅 测量 距离 ;但 它 不 会 直接 对 它们 做 出 反应 。 
距离 计算 模块 允许 记录 作为 可 测量 实体 对 的 距离 对 象 。 在 仿真 期 间 , 每 个 注册 的 距离 
对 象 的 最 小 距离 段 可 以 被 可 视 化 或 记录 在 图 形 对 象 中 。 如 果 注 册 距 离 对 象 的 两 个 合成 实体 
在 复制 粘贴 操作 中 同时 复制 , 则 注册 的 距离 对 象 也 会 自动 复制 。 距 离 计 算 对 话 框 是 计算 模 
块 属性 对 话 框 的 一 部 分 ,可 以 通过 依次 单 击 菜单 栏 , 工 具 、 计 算 模 块 属性 来 打开 。 

在 计算 模块 属性 对 话 框 中 , 单 击 距 离 计 算 按 钮 以 显示 距离 计算 对 话 框 ,如 图 3-54 所 示 。 


Calculation Modules [=] 


[ Colson detec. | [Distance cake. | [Path planning | { Moton plann. 
| Inverse kinematics | | Geometric constraint solver | | Dynamcs | 


[V] Enable distance calculations. 
| Mddnewdetame object — 


图 3-54 距离 计算 对 话 框 


启用 所 有 距离 计算 : 允许 启用 或 禁用 所 有 注册 距离 对 象 的 距离 计算 。 

添加 新 距离 对 象 : 允许 指定 两 个 实体 进行 距离 计算 。 按 钮 下 方 的 列表 显示 所 有 注册 的 
距离 对 象 , 双 击 它们 可 以 重 命名 。 可 以 选择 列表 中 的 单个 距离 对 象 , 然 后 在 下 面 显 示 相 关 属 
性 ( 见 下 文 )。 

显 式 处 理 : 指示 是 否 应 显 式 处 理 所 选 的 距离 对 象 。 如 果 选 中 , 当 调 用 simHandleDistance 
(sim_handle_all_except_explicit) 时 ,但 仅 当 调用 simHandleDistance(sim_handle_all) 或 
simHandleDistance(distanceObjectHandle) 时 .不 会 处 理 此 距离 对 象 的 距离 计算 。 如 果 用 
户 希 望 在 子 脚本 而 不 是 在 主 脚本 中 处 理 该 距离 对 象 的 距离 测量 ,这 将 是 有 用 的 (如 果 未 选 
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中 , 则 该 距离 对 象 的 距离 计算 将 被 处 理 两 次 ,一 次 是 在 主 脚本 调用 simHandleDistanceCsim | 
handle all except explicit) Hif , 另 一 次 是 在 子 脚本 中 调用 simHandleDistance(distanceOb- 
jectHandle) 时 。 

EARE: 当 实 体 间 的 距离 很 远 并 且 不 需要 计算 距离 时 ,可 以 指定 距离 冰 值 用 于 加 速 
计算 。 

显示 距离 段 : 如 果 启 用 , 则 该 距离 对 象 的 最 小 距离 段 在 场景 中 可 见 。 

段 宽度 : 距离 段 的 宽度 。 

段 颜色 : 允许 调整 距离 段 的 颜色 。 


3.5.3 逆向 运动 学 模块 


1. 逆向 运动 学 模块 的 功能 

正 向 和 逆向 运动 学 模块 : 它 支持 分 支 .闭环 、 柑 套 循环 等 多 种 类 型 的 运动 学 计算 机 制 。 
该 模块 是 基于 阻尼 最 小 二 乘法 伪 逆 的 计算 方法 , 它 支 持 有 限制 条 件 的 解决 方法 、 阻 尼 和 加 权 
决议 的 方法 ,以 及 基于 约束 的 避 障 的 方法 。 

V-REP 的 逆 运 动 学 (IK) 计 算 模块 非常 强大 和 灵活 。 它 允许 在 逆 运 动 学 模式 (IK 模式 ) 
或 正 向 运动 模式 (FK 模式 ) 中 人 处理 几乎 任何 类 型 的 机 制 。IK 的 问题 可 以 看 作 是 寻找 关节 值 
的 问题 ,而 这 个 关节 值 取决 于 给 定 主体 元 件 ( 通 常 是 末端 执行 器 ) 的 具体 位 置 和 方向 。 一 般 
地 说 , 它 是 从 任务 空间 坐标 到 关节 空间 坐标 的 变换 。 例 如 ,对 于 串 行 操纵 器 ,需要 解决 的 问 
题 是 在 给 定 末 端 执行 器 的 位 置 ( 和 /或 取向 ) 的 情况 下 找到 操纵 器 中 的 所 有 关节 的 值 。 与 之 
相反 ,找到 给 出 关节 值 的 末端 执行 器 位 置 则 被 称 为 FK 问题 ,并 且 通 常 被 认为 是 比 IK 更 容 
易 的 任务 。 在 处 理 开放 运动 链 时 ,FK 问题 确实 比 IK 问题 容易 ,但 是 对 于 一 般 类 型 的 机 械 
配置 则 不 是 这 样 。 

道 运动 学 计算 模块 的 一 些 结果 可 以 通过 图 形 对 象 来 记录 。 运 动 学 功能 还 可 用 于 外 部 应 
用 ( 即 ,不 直接 是 V-REP 框架 的 一 部 分 的 应 用 ,例如 在 不 同 计算 机 上 ,在 机 器 人 或 控制 器 上 
的 应 用 )。 这 些 都 可 以 通过 外 部 运动 学 功能 实现 。 最 后 ,请 确保 查看 文件 夹 场 景 / dk fk 
simple examples 中 与 IK 和 FK 相关 的 各 种 简单 示例 场景 。 

2. IK 组 和 IK 元 素 的 基础 知识 

V-REP 使 用 IK 组 和 IK 元 素来 解决 道 向 和 正 向 运动 学 任务 。 重 要 的 是 要 了 解 如 何 解 
决 IK 任务 ,以 充分 利用 该 模块 的 功能 。IK 组 包含 一 个 或 多 个 IK 元 素 : 

(1) IK H: IK 组 将 一 个 或 多 个 IK 元 素 组 合 。 要 解决 简单 运动 链 的 运动 学 ,需要 一 个 
包含 一 个 IK 元 素 的 IK 组 。IK 组 定义 了 一 个 或 多 个 IK 元 素 的 总 体 求解 属性 (例如 要 使 用 
什么 求解 算法 等 ) 。 

(2) IK 元 素 : IK 元 素 指 定 简单 的 运动 链 。 一 个 IK 元 素 表 示 一 个 运动 链 。 和 运动 链 是 包 
含 至 少 一 个 联合 对 象 的 连 杆 。 简 而 言 之 ,IK 元 素 由 以 下 组 成 : 

基 座 (任何 类 型 的 物体 ,甚至 是 关节 ,在 这 种 情况 下 .关节 被 认为 是 刚性 的 ): 它 代表 运 
动 链 的 开始 。 
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几 个 连 杆 (任何 类 型 的 对 象 ,除了 关节 ): 然而 ,不 在 IK 模式 中 的 关节 也 被 认为 是 连 杆 
(在 这 种 情况 下 ,它们 表现 为 刚性 关节 (固定 值 ))。 

几 个 关节 : 然而 ,不 在 IK 模式 中 的 关节 不 被 认为 是 关节 ,而 是 作为 连 杆 (参见 上 文 )。 

一 个 尖端 : 尖端 始终 是 虚拟 的 ,并 且 是 所 考虑 的 运动 链 中 的 最 后 一 个 对 象 ( 当 从 基部 到 
尖端 时 )。 尖 端 虚拟 器 应 连 杆 到 目标 虚拟 器 ( 见 下 文 ) ,并 且 链 路 应 为 IK ,尖端 -目标 链 路 
类 型 。 

一 个 目标 : 目标 始终 是 虚拟 的 ,并 且 代表 尖端 在 仿真 期 间 应 当 采 用 (或 跟随 ?的 位置 和 / 
或 方向 。 目 标 虚 拟 体 应 该 连 杆 到 尖端 虚拟 (参见 上 文 ) 上 ,并 且 连 杆 应 该 是 IK 或 尖端 -目标 

IK 元 素 由 运动 链 指定 ,并 且 指 点 跟随 目标 。 运 动 链 本 身 由 工具 提示 (或 末端 执行 器 ,或 
简称 末端 ,和 一 个 基础 设置 指定 。 工具 提示 链 中 的 最 后 一 个 对 象 而 基础 设置 指示 链 中 的 基 
础 对 象 (或 第 一 对 象 )。 如 图 3-55 所 示 , 显 示 了 为 TK 元 素 指定 的 两 个 运动 链 。IK 元 素 以 类 
似 的 方式 感知 两 个 链 ( 第 二 个 示例 的 第 一 个 关节 被 IK 元 素 忽 略 )。 


图 3-55 ”两 个 运动 链 (各 自 描述 一 个 IK 元 素 ) 


在 上 述 示例 中 ,由 尖端 /基部 对 指定 的 运动 链 都 具有 3 自由 度 (DoF), 因 为 涉及 3 个 
1-DoF 关节 。 然 而 ,如 果 关 节 中 的 一 个 是 球形 关节 , 则 链 将 具有 5DOF ,因为 球形 关节 自身 
具有 3DOF 。 现 在 你 应 该 告诉 IK 元 素 指定 的 运动 链 在 仿真 过 程 中 应 该 如 何 表现 。 通 常 , 如 
图 3-56 所 示 ,我 们 希望 运动 链 的 尖端 跟随 目标 (参考 虚拟 属性 以 形成 尖 - 目 标 连 杆 ) 。 


ik 元 任务 IK 组 (IK group) 


图 3-56 IK ERA IK 解决 任务 的 相应 模型 
当 仿真 运行 并 且 一 些 附 加 参数 已 被 正确 定义 (参见 下 文 参数 描述 ) 时 , 则 机 构 ( 指 定 的 运 


动 链 ) 应 当 朝 向 目标 移动 。 这 是 IK 任务 的 最 基本 情况 。 两 个 单独 的 运动 链 以 相同 的 方式 
处 理 , 但 是 这 次 需要 两 个 IK 组 (并 且 每 个 组 都 应 该 包含 每 个 运动 链 的 一 个 IK 元 素 )。 解 决 
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两 个 IK 组 的 顺序 并 不 重要 。 
如 图 3-57 所 示 ,在 上 面 的 例子 中 ,如 果 目 标 2 被 附加 到 第 一 运动 链 的 移动 部 分 , 则 求解 顺 
序 变 得 重要 ,并 且 应 当 首先 求解 斑 组 1( 求 解 结果 将 移 位 目标 2, 如 从 图 3-58 可 以 看 出 的 ): 


目标 2( 依 附 于 链 1) 
顶端 2 


顶端 1 
顺序 解 
IKJEXOK element2) 


图 3-57 ”两 个 单独 的 TK 链 ,其 中 第 二 链 的 目标 连接 到 第 一 链 , 以 及 IK 解决 任务 的 相应 模型 


当 一 个 IK 元 素 建 立 在 另 一 个 IK 元 素 的 顶部 而 没有 共享 任何 公共 关节 时 ,可 以 出 现 类 
似 的 情况 ,如 图 3-58 所 示 。Base2 是 两 条 链 之 间 的 共同 对 象 。 解 决 IK element? 不 会 替换 
Base2 ,但 是 解析 IK elementl 将 会 替换 它 。 因 此 ,如 图 3-58 所 示 ,IK groupl 必须 在 IK 
group2 之 前 解决 ,如 上 面 的 情况 (求解 顺序 很 重要 )。 


顺序 解 


图 3-58 两 个 IK 链 共享 一 个 公共 连 杆 ,但 没有 公共 关节 和 IK 解决 任务 的 相应 模型 


当 两 个 或 更 多 个 运动 链 共享 公共 关节 时 出 现 更 困难 的 情况 。 在 这 种 情况 下 ,顺序 求解 
在 大 多 数 时 间 不 工作 (在 下 面 的 示例 中 ,两 个 IK 元 素 倾向 于 将 公共 关节 旋转 到 相反 的 方 
向 ) ,并 且 需 要 同时 求解 方法 。 为 了 同时 求解 几 个 IK 元 素 ,只 需 将 它们 组 合成 一 个 公共 的 
IK 组 。 这 种 情况 如 图 3-59 所 示 。 


顶端 1 顶端 2 
目标 ! Oc 
IK element 任 务 1 
公共 关节 


IK element 任 务 2 


Base 1 


图 3-59 两 个 IK 链 共享 一 个 共同 的 关节 和 IK 解决 任务 的 相应 模型 
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3. 解决 任何 类 型 的 机 制 的 IK 和 FK 问题 

通过 使 用 IK 组 和 IK 元 素来 解决 反 向 运动 学 (IK) 或 正 向 运动 学 (FK) 的 机 制 , 记 住 以 
下 检查 点 ,以便 成 功 设置 IK 或 FK 计算 : 

CD 通过 提供 基 座 和 尖端 来 指定 单个 运动 链 。 

(2) 指定 要 跟踪 的 目标 (简单 地 将 尖端 虚拟 对 象 连接 到 目标 虚拟 对 象 ) 。 

(3) 如 果 它 们 共享 公共 关节 , 则 IK 组 元 素 在 单个 IK 组 中 。 

(4) 订购 单个 TK 组 以 获取 所 需 的 行为 。 

(5) 验证 运动 链 中 的 关节 是 否 启用 或 禁用 了 正确 的 属性 。 

(6) 验证 单个 IK 元 素 不 是 过 约束 的 (X,Y,Z,Alpha-Beta,Gamma)。 

最 后 一 点 是 非常 重要 的 : -MEDERI zey 
开 所 有 的 约束 ,尖端 将 跟随 其 相关 的 目标 在 x,y,z 方 ”项 端 xk ÈO nu 
向 ,同时 试图 保持 与 目标 相同 的 方向 。 然 而 ,这 仅 当 一 一 
运动 链 具 有 至 少 6 个 非 宛 余 自 由 度 (DoF) 时 才 有 效 。 
尖端 应 始终 被 适当 地 约束 ( 即 从 未 指示 比 在 机 制 中 存 
在 DoF 更 多 的 约束 )。 如 图 3-60 所 示 ,位 置 约束 多 半 
是 相对 于 基本 方向 指定 的 。 

有 时 我 们 并 不 能 够 正确 地 指定 为 一 个 尖端 的 限 
制 ,并且 在 这 种 情况 下 ,IK 组 的 计算 方法 应 该 是 一 个 
含有 适当 可 选 的 阻尼 因子 的 阻尼 方法 (例如 DLS 
法 ) 。 当 我 们 可 能 无 法 到 达 目标 (不 可 达到 ,或 接近 单 图 3-60 运动 链 的 位 置 约束 
个 配置 ) 时 ,应 选择 阻尼 分 辩 率 方法 。 阻 尼 方 法 可 以 
使 得 计算 更 稳定 ,但 需要 注意 的 是 ,阻尼 方法 也 会 使 得 IK 计算 速度 减 慢 ( 需 要 更 多 的 迭代 
将 尖端 放置 到 位 ) 。 

打开 Alpha-Beta 约束 将 使 尖端 的 = 轴 方 向 与 目标 的 z 轴 方 向 匹配 ,同时 如 果 Gamma 
约束 关闭 , 则 可 以 让 尖端 围绕 = 轴 自 由 地 旋转 。 当 Alpha-Beta 约束 和 Gamma 约束 打开 时 ， 
尖端 将 尝试 保持 与 其 关联 目标 完全 相同 的 方向 。 

如 何 解 决 IK 简单 运动 链 的 IK 问题 在 有 关 IK 组 和 IK 元 素 基 础 的 章节 有 解释。 解决 
简单 运动 链 的 FK 问题 是 容易 的 (只 是 将 所 需 的 关节 值 应 用 于 链 中 的 所 有 关节 ,以 获得 尖端 
或 末端 执行 器 的 位 置 和 方向 )。 但 是 解决 闭合 机 制 的 IK 和 FK 问题 并 不 简单 。 

对 于 FK 问题 ,首先 要 确定 想 控 制 的 关节 (如 驱动 机 构 的 关节 ,活动 关节 )。 那 些 关 节 应 
该 从 所 有 运动 学 计算 中 排除 (选择 一 个 不 是 反 向 运动 模式 的 关节 (参见 关节 属性 ))。 从 现在 
起 ,那些 关节 将 通过 运动 学 计算 被 认为 是 刚性 的 。 然 后 ,识别 哪个 运动 链 需要 关闭 。 闭 合 
操作 将 由 尖端 -目标 对 组 成 的 闭合 约束 环 来 处 理 , 如 图 3-61 所 示 。 然 后 ,为 活动 关节 设置 
期 望 的 关节 值 , 并 调用 逆 运 动 学 功能 来 处 理 闭 环 约束 (默认 主 脚本 处 理 未 标记 为 显 式 处 理 
的 所 有 IK 组 )。 下 面 的 例子 显示 了 一 些 可 用 于 解决 复杂 运动 学 问题 的 附加 功能 ,如 图 3-62 
BUR S 
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环 闲人 台 约 束 3、 


环 闭合 约 东 1、、 一 环 闭合 约 东 2 


I 组 1 
EH)... 
活动 关节 2 
图 3-61 封闭 机 构 正 向 运动 学 求解 方法 


从 图 3-62 可 以 看 出 ,用 户 有 一 个 IK 任务 : 将 尖端 放 到 目标 上 (或 使 尖端 跟随 目标 ) 。 
这 可 以 以 常规 方式 解决 。 或 者 用 户 可 以 如 图 3-63 所 示 , 使 用 关节 依赖 性 功能 。 


-0 
一 DO 
[N ten KEL 
jatii ea 
环 闭 台 约束、 了 9 xu 
E 约束 
图 3-62 ” 道 运 动 学 任务 图 3-63 具有 关节 重 蚕 约束 的 道 运动 学 任务 


IK 主要 任务 是 负责 让 尖端 到 达 目 标 , 闭 环 约 束 负责 关闭 机 制 ( 合 并 杆 机 构 ) ,并且 关节 
重合 约束 负责 保持 机 构 的 基础 固定 (成 为 一 条 链 )。 必 须 仔细 选择 关节 依赖 性 线性 方程 的 参 
数 ,以 便 达 到 完美 接合 ,例如 ,如 果 两 个 对 应 的 关节 (通过 重 琶 约束 连 杆 的 关节 ) 具 有 相同 的 
方向 , 则 方程 中 的 系数 需要 设置 成 一 1。( 因 为 一 个 关节 是 自 下 而 上 构造 的 ,而 另 一 个 关节 是 
自 上 而 下 构造 的 。) 

4. 反 向 运动 学 对 话 框 的 设置 

逆 运 动 学 对 话 框 是 计算 模块 属性 对 话 框 的 一 部 分 ,可 以 通过 依次 单 击 菜单 栏 . 工 具 、. 计 
算 模 块 属性 来 打开 。 

在 计算 模块 属性 对 话 框 中 , 单 击 反 向 运动 学 按钮 以 显示 反 向 运动 学 对 话 框 ,如 图 3-64 
所 示 。 

启用 反 向 运动 : 启用 或 禁用 所 有 反 向 运动 学 计算 。 

添加 新 IK 组 : 添加 一 个 新 的 空 IK 组 。IK 组 可 以 包含 一 个 或 多 个 IK 元 素 。IK 元 素 是 
基本 运动 链 的 IK 任务 IK 组 可 以 对 整 组 元 素 同 时 进行 求解 。 仅 在 需要 时 使 用 同时 求解 ( 计 
算 时 间 比 顺序 求解 更 长 ) 。IK 元 素 必须 始终 与 IK 组 相关 联 , 并 且 不 能 单独 存在 。 按 钮 下 方 
的 列表 显示 了 在 IK 计算 期 间 所 有 需要 求解 的 TK 组 。 选 择 列表 中 的 IK 组 ,可 以 在 对 话 框 
的 剩余 部 分 显示 该 组 的 参数 。 列 表 中 的 顺序 很 重要 (IK group2 的 快速 并 正确 地 执行 可 能 
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Calculation Modules Bn 
[Colsondetec. | | Distance cak. | | Path plarring | Moton plann. | 
Limverseknematcs | | Geometicconstantssver | | Dymmes | 


[e] [8] 


图 3-64 道 向 运动 学 对 话 框 


依赖 于 IK groupl 的 运行 结果 )。 列 表 旁 边 的 两 个 按钮 允许 更 改 所 选 TK. 组 的 位 置 。 

IK 组 处 于 活动 状态 : 允许 打开 和 关闭 单个 IK 组 。 

显 式 处 理 : 指示 是 否 应 显 式 处 理 所 选 的 IK 组 。 如 果 选 中 , 当 调 用 simHandleIkGroup 
(sim_handle_all_except_explicit) 时 ,但 仅 当 调用 simHandlelkGroup(sim handle all) 7X 
simHandleIkGroup(ikGroupHandle) 时 ,将 不 处 理 此 IK 组 的 IK 计算 。 如 果 用 户 希 望 在 子 
脚本 而 不 是 主 脚本 中 处 理 该 IK 组 的 运动 学 问题 (如 果 该 选项 未 选中 , 则 该 TK. 组 将 进行 两 
次 IK 计算, 一 次 是 在 主 脚本 调用 simHandleIkGroup(sim_handle_all_except_explicit) 时 ， 
另 一 次 是 在 子 脚本 中 调用 simHandleIkGroup(ikGroupHandle) 时 。 

机 构 是 多 余 的 : 当选 择 时 .在 TK 解 算 期 间 将 应 用 关节 限制 修正 。 否 则 ,在 IK 解 算 之 后 
将 简单 地 强制 执行 关节 限制 ,这 可 能 导致 不 稳定 。 

忽略 最 大 步 长 : 如 果 选 中 此 属性 ,将 忽略 关节 属性 中 指定 的 最 大 步 长 。 

计算 值 方法 : 用 于 指定 IK 组 解 算 的 计算 方法 。 伪 逆 是 最 快 的 方法 ,但 是 当 目标 和 尖端 
相距 太 远 时 ,或 者 在 运动 链 过 度 约束 时 又 或 者 在 机 构 接近 单个 配置 或 目标 不 可 达 时 ,其 可 能 
是 不 稳定 的 。DLS 较 慢 ,但 更 稳定 ,因为 它 是 一 种 阻尼 解 算 方 法 (可 指定 阻尼 系数 (阻尼 )) 。 
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当 伪 逆 方 法 可 能 失败 时 ,这 将 是 一 个 好 的 选择 。 

阻尼 : 使 用 阻尼 解 算 方 法 (DLS) 时 的 阻尼 系数 。 当 阻尼 系数 大 时 ,求解 将 更 稳定 但 也 
更 加 缓慢 。 关 键 是 要 调整 到 合适 的 值 。 

最 大 和 迭代 次 数 : 可 以 指定 最 大 迁 代 次 数 。 这 是 给 定 IK 组 的 最 大 计算 次 数 ,直到 达到 其 
指定 的 解 的 精度 。 阻 尼 求 解 方法 (DLS) 通 常 比 非 阻尼 求解 ( 伪 逆 ) 需 要 更 多 的 迭代 。 

关节 限制 (计算 权重 ) : 应 用 于 关节 限制 约束 的 计算 权重 (关节 限制 约束 在 关节 属性 (位 
置 最 小 值 和 位 置 范围 ) 中 指定 ) 。 

障碍 物 回 避 ( 计 算 权重 ) : 应 该 用 于 障碍 物 回避 约束 的 计算 权重 。 

关节 限制 阔 值 : 应 与 关节 限制 约束 一 起 使 用 的 线性 和 角 阔 值 。 

编辑 条 件 参数 : 允许 调整 所 选 IK 组 的 条 件 解 析 参 数 。 将 弹出 IK 条 件 解析 对 话 框 ,如 
图 3-65 所 示 。 

执行 .…: 这 是 条 件 解 析 部 分 。 用 户 可 以 在 下 拉 列 表 中 选择 IK 组 ,其 IK 解析 结果 将 决 
定 当 前 IK 组 是 否 将 被 解决 。 当 IK 元 素 都 在 其 指定 的 线性 / 角 精 度 内 将 被 认为 是 成 功 的 IK 
组 计算 。 

如 果 … 恢 复 : 如 果 解 算 不 成 功 (未 达到 位 置 和 /或 方向 精度 ), 则 允许 恢复 初始 TK 组 配 
置 ( 关 节 值 )。 结 合 上 述 条 件 求解 ,用 户 可 以 组 合 两 种 不 同 的 计算 方法 。 这 在 机 械 手 的 目标 
可 能 不 可 达 或 接近 奇 点 的 情况 下 是 有 用 的 : 第 一 次 尝试 利用 非 阻 尼 分 辨 率 方法 ( 伪 逆 , 快 
速 ) 来 解决 TK. 组 ,如 果 不 成 功 , 则 第 二 次 则 尝试 使 用 阻尼 分 辨 率 方法 (DLS, 较 慢 )。 用 户 当 
然 也 可 以 从 脚本 ,插件 中 “手动 ”处 理 IK 解析 。 

编辑 障碍 物 回避 参数 : 允许 调整 障碍 物 回避 参数 。 请 注意 ,这 只 对 宛 余 的 机 械 臂 有 意 
义 , 并 且 仿 真 速度 可 能 会 急剧 下 降 。 将 弹出 IK 避 障 对 话 框 ,如 图 3-66 所 示 。 


IK Group Conditional Parameters 


Perform if... IK Obstacle Avoidance 
(4360 manTask 7| [was performed and faled z) ER 
Restore if .... 


Entity 1: Manipulator links [Collection] 


C] near precision requirements were not fulfilled 


Enety 2: al other measurable objects in the scene 
C Angular precision requirements were not fulfilled. 


Distance threshold [m] (0.000 | 


图 3-65 IK 条 件 解 析 对 话 框 图 3-66 IK 避 障 对 话 框 


选择 回避 实体 /删除 回避 约束 : 允许 选择 /删除 回避 实体 。 一 个 实体 通常 是 机 器 人 , 另 
一 个 实体 是 障碍 物 。 使 用 简单 的 可 测量 对 象 快 速 求 解 。 

EARE: 相互 避 开 的 实体 之 间 应 保持 的 最 小 距离 。 

5. IK 元 素 对 话 框 的 设置 

IK 元 素 对 话 框 是 逆 运 动 学 对 话 框 的 一 部 分 。 该 对 话 框 显示 给 定 TK 组 的 各 种 IK 元 素 。 
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在 道 运动 学 对 话 框 中 ,选择 IK 组 ,然后 单 击 编辑 IK 元 素 按钮 以 打开 IK 元 素 对 话 框 ,如 
图 3-67 所 示 。 


IK Elements 


| Add new K element with tp: — Resizablefloor 5 25 zj 


Corstrants 

Bx Wr Mz  [ViAphabeta [7] Gamma 
Relative to coordinate frame |Sameasbase 7 
Desred preasons Resolution wesghts 


图 3-67 IK 元 素 对 话 框 


添加 带 有 末端 的 新 IK 元 素 : 使 用 默认 值 添加 一 个 新 的 IK 元 素 ( 由 其 顶点 虚拟 定义 ) 。 
IK 组 可 以 包含 多 个 IK 元 素 ,它们 显示 在 列表 中 。 列 表 中 的 IK 元 素 将 在 TK 计算 期 间 同 时 
求解 (因为 它们 在 同一 IK 组 中 ) .在 列表 中 的 顺序 并 不 重要 。 在 列表 中 选择 IK 元 素 将 显示 
其 属性 和 人 参数。 

元 素 处 于 活动 状态 : 允许 打开 和 关闭 所 选 的 IK 元 素 。 无 效 的 IK. 元 素 可 能 在 仿真 期 间 
自动 关闭 。 

基础 : IK 链 的 基础 对 象 。 请 记 住 .如 果 没 有 另外 指定 , 则 相对 于 基础 对 象 的 方向 指定 
Zw. 

目标 : 尖端 应 该 遵循 的 虚拟 对 象 。 目标 不 应 该 是 指定 的 运动 链 的 一 部 分 (否则 IK 元 素 
的 求解 可 能 无 法 完成 ) ,但 是 建议 在 基础 之 上 构建 目标 (保持 一 个 干净 的 场景 层次 结构 )。 如 
果 选 定 的 尖端 虚拟 对 象 未 连 杆 到 任何 其 他 虚拟 对 象 ,或 者 连 杆 类 型 不 是 IK、tip-target, 则 将 
显示 警告 消息 ,而 不 是 目标 虚拟 名 称 。 

约束 : 在 IK 元 素 求解 期 间 需 要 遵守 的 约束 条 件 。 默 认 情 况 下 相对 于 基础 指定 位 置 约 
东 , 但 可 以 选择 另 一 个 虚拟 对 象 作为 约 东 参考 框架 (相对 于 坐标 框架 项 )。 过 度 约 东 的 TK 
元 素 将 不 能 正确 求解 并 且 不 稳定 。 仔 细 分 析 需 要 什么 约束 以 及 相对 于 什么 参考 框架 是 很 重 
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要 的 。 然 而 ,有 时 难以 不 过 度 地 约束 IK 元 素 。 在 这 种 情况 下 ,应 该 为 IK 组 选择 阻尼 求解 
方法 (DLS) ,并 适当 调整 阻尼 系数 。 但 请 记 住 ,阻尼 求解 较 慢 。(X,Y 和 Z 对 应 的 是 位 置 
(尖端 将 跟随 目标 的 位 置 ), Alpha-Beta 和 Gamma 对 应 的 是 方向 (尖端 将 跟随 目标 的 
方向 )) 。 

线性 /角度 精度 : 指定 期 望 的 线性 /角度 精度 。 如 果 尖 端 在 目标 的 线性 和 角度 精度 内 
(考虑 特定 的 约束 ), 则 认为 IK 元 素 可 求解 。 

线性 /角度 权重 : 用 于 位 置 或 方向 求解 的 IK 求解 权重 。 用 户 可 以 为 IK 元 素 的 求解 先 
择优 先进 行 位 置 跟随 还 是 方向 跟随 。 这 对 于 宛 余 机 制 尤 其 有 用 。 


3.5.4 几何 约束 求解 模块 


几何 约束 求解 模块 允许 解决 逆向 或 前 向 运动 的 问题 。V-REP 的 逆 运 动 学 计算 模块 非 
常 强 大 和 灵活 。 如 果 使 用 正确 , 它 允 许 以 快速 和 准确 的 方式 解决 任何 类 型 的 正 向 / 反 向 运动 
问题 。 然 而 ,由 于 用 户 的 机 械 结 构 或 运动 学 知识 水 平 不 同 , 该 模块 可 能 是 难以 使 用 的 。 另 一 
方面 ,几何 约束 求解 器 在 求解 运动 学 问题 时 速度 较 慢 ,精度 较 低 ,但 使 用 起 来 可 能 更 容易 和 
更 直观 。 此 外 ,其 允许 以 比 逆 运 动 学 计算 模块 更 灵活 的 方式 与 机 构 交 互 。 几 何 约 束 求解 器 
的 一 些 结果 可 以 由 图 形 对 象 记录 。 

V-REP 的 几何 约束 求解 器 功能 以 与 运动 学 计算 模块 类 似 的 方式 操作 ,差别 在 于 求解 器 
将 尝试 自动 识别 运动 链 , 并 以 适当 的 方式 (如 自动 约束 调整 ,循环 闭合 (或 闭环 ) 等 ) 处 理 
它们 。 

通常 ,用 户 必须 告诉 求解 器 : 哪些 对 象 (虚拟 对 象 ) 应 该 重合 (以 便 例如 关闭 循环 ); 什 
么 机 制 必 须 由 几何 约束 求解 器 处 理 ; 哪些 附加 约束 应 该 应 用 于 该 机 制 。 


1. 指定 闭 包 约束 几何 约束 求解 器 一 一 < 
闭 包 约束 可 以 被 看 作 需要 两 个 对 象 的 配置 (位 置 和 vs / C 
N 


取向 相同 的 约束 ,如 图 3-68 所 示 ( 为 了 清楚 起 见 ,表示 Ty oot ” 
限于 二 个 维度 ) 。 ~ 对 象 1 

几何 约束 求解 器 将 尝试 重 琶 试图 保持 机 构 一 致 的 两 
个 对 象 的 位 置 和 取向 ( 即 , 仅 调整 机 构 中 的 IK 模式 中 的 
关节 以 达到 该 重 释 情况 , 见 图 3-69)。 几 何 约 束 求解 程序 用 于 指定 闭 包 约束 的 对 象 是 虚拟 
的 。 为 此 ,两 个 虚拟 需要 标记 为 闭合 对 。 这 可 以 通过 选择 相反 的 虚拟 为 连 杆 虚拟 ,并 且 指 定 
GCS 和 链 路 类 型 的 重 释 约 束 在 虚拟 属性 中 调整 。 如 图 3-70 所 示 , 它 说 明了 指定 几何 约束 求 
解 器 闭 包 约 束 的 两 个 连 杆 的 虚拟 对 象 。 

2. 指定 要 解决 的 机 制 

接 下 来 ,几何 约束 求解 器 需要 被 告知 它 必 须 处 理 哪个 机 制 。 当 被 认为 是 一 个 相同 的 机 
制 时 ,所 有 对 象 可 以 从 给 定 的 对 象 到 达 , 通 过 遵循 一 个 路 径 可 以 去 : 

。 从 一 个 对 象 到 其 父 对 象 。 

。 从 一 个 对 象 到 其 子 对 象 。 


对 象 2 


图 3-68 几何 约束 求解 器 闭 包 约束 
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f E 
n C 
" H n 本 ~, 
-A TI M 对 象 !/ 和 对 象 2 
(a) 中 同情 况 (b) 最 终 情况 


图 3-69 几何 约束 求解 器 的 闭合 动作 


3-70 ”两 个 连 杆 的 虚拟 对 象 指定 一 个 闭 包 约束 


。 从 一 个 虚拟 到 其 连 杆 虚拟 (但 是 只 有 GCS 类 型 的 连 杆 , 重 全 约束)。 

如 果 两 个 连 杆 的 虚拟 项 连接 它们 , 则 不 共享 任何 公共 父 对 象 (以 下 称 为 元 素 ) 的 两 个 对 
象 树 也 可 以 是 相同 机 制 的 一 部 分 。 对 于 两 个 不 共享 任何 公共 父 对 象 的 对 象 树 ,如 果 两 个 连 
杆 的 虚拟 项 连接 它们 ,它们 也 可 以 成 为 相同 的 机 构 的 一 部 分 ,如 图 3-71 所 示 。 
装置 1， 元 素 1 


虚拟 连 杆 
装置 1， 元 素 3 


-虚拟 连 杆 
装置 1， 元 素 2 


图 3-71 由 3 个 元 素 (对 象 树 ) 组 成 的 一 个 机 制 


选择 作为 机 制 探 索 (路 径 探索 ) 的 起 点 的 对 象 的 最 后 一 个 父 对 象 被 称 为 机 制 的 基本 对 
象 。 当 几何 约 东 求解 器 试图 解决 一 个 机 制 时 , 它 将 尝试 通过 保持 机 制 的 基本 对 象 来 实现 。 
记 住 它 是 很 重要 的 。 想 要 指定 需要 解决 的 机 制 ,请 选择 一 个 机 构 基础 对 象 的 父 对 象 ,然后 在 
几何 约 东 解 算 器 对 话 框 中 单 击 * 插 入 新 对 象 "。 一 个 相同 的 机 制 在 求解 中 只 能 被 注册 一 次 。 
在 IK 模式 中 ,至 少 要 包括 一 个 关节 的 机 制 才 会 被 求解 器 处 理 。 

3. 指定 其 他 约束 

可 以 为 机 制 指定 其 他 位 置 约 束 。 这 可 以 通过 形成 尖端 -日 标 对 (指定 GCS, 提 示 /GCS， 
目标 作为 虚拟 属性 中 的 连 杆 类 型 ) 的 两 个 连 杆 的 虚拟 


虚拟 连 杆 


几何 约束 求解 器 

对 象 来 实现 。 如 图 3-72 所 示 , 它 给 出 了 两 个 连 杆 的 虚 wp 

拟 对 象 ,其 中 一 个 被 标记 为 目标 (为 了 清楚 起 见 . 仅 限 * ¢ 

于 二 维 )。 s -虚拟 体 2 _ 
虚拟 体 1 《标记 为 目标 ) 


被 标记 为 目标 的 虚拟 器 不 被 认为 是 机 制 的 一 部 
分 ,因此 在 几何 约束 解析 期 间 将 不 移动 ,而 另 一 虚拟 器 图 3 72 几何 约束 求解 器 位 置 约束 
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将 试图 到 达 被 标记 为 目标 的 虚拟 器 所 处 的 位 置 , 见 图 3-73。 可 以 在 运动 学 计算 模块 中 使 用 
尖端 -目标 虚拟 对 来 进行 类 比 。 如 图 3-74 Bros ,说 明了 两 个 连 杆 的 虚拟 对 象 ,其 中 一 个 标记 


为 目标 。 
emk 
> 
ond, 
Ne Ned O 
C gne G me i 
ERE (标记 为 目标 (标记 为 目标 ) 人 虚拟 体 2 
ax 
人 中 间 情 况 人 最 终 情况 虚拟 体 1 。 《标记 为 目标 ) 
图 3-73 几何 约束 求解 器 的 位 置 约束 动作 图 3-74 两 个 连 杆 的 虚拟 对 象 ， 


其 中 一 个 被 标记 为 目标 


4. 几何 约束 求解 器 操作 

当 构 建 将 由 几何 约束 求解 器 解决 的 机 制 时 ,确保 机 制 是 一 致 的 ,并 且 约 束 是 可 能 的 ( 即 ， 
机 制 具有 足够 的 自由 度 )。 

在 仿真 期 间 ,当选 择机 械 导航 模式 时 ,可 以 用 鼠标 以 灵活 的 方式 来 操纵 先前 注册 要 用 几 
何 约束 求解 器 求解 的 机 制 。 该 模式 可 以 通过 以 标记 为 目标 的 虚拟 体 
下 API 调用 启用 : simSetNavigationMode(sim_ ( 非 装置 部 分 ) 
navigation_ikmanip) 只 需 单 击 并 拖 动 任何 在 机 Q RHE 
制 中 的 形状 对 象 , 解 算 器 将 尝试 考虑 该 额外 的 位 
HAR. WE 3-75 所 示 , 它 说 明了 这 种 拖 动 操 
作 : 将 机 制 构 建 为 树 结构 ( 即 , 机 制 中 的 所 有 对 象 
具有 至 少 一 个 共同 的 父 对 象 ), 让 具有 连 杆 的 虚 
拟 项 负责 关闭 某 些 树 分 支 ,这 些 都 是 好 的 习惯 ， MÀ 
可 以 减少 机 制 的 复杂 性 ,简化 机 制 的 场景 层次 结 图 3-75 具有 两 个 附加 位 置 约束 的 机 制 
构 表示 ,并 且 我 们 将 能 够 将 该 机 制作 为 模型 处 
理 。 几 何 约 束 求解 器 功能 也 非常 适合 于 简单 机 制 ,并且 能 够 实现 与 用 户 的 直接 和 灵活 的 交互 。 


3.5.5 动力 学 模块 


动力 学 或 物理 模块 以 V-REP 内 置 的 四 个 物理 引擎 作为 支撑 ,允许 处 理 刚体 动力 学 的 
计算 和 交互 (碰撞 响应 . 抓 取 等 )。 动 力学 或 物理 模块 已 默认 包含 在 V-REP 的 仿真 环境 中 ， 
直接 作用 于 所 有 启用 动态 的 场景 对 象 。 例 如 ,在 场景 中 添加 一 个 与 地 板 有 一 定 高 度 距离 的 
小 球 ,并 把 它 设置 为 动态 ,仿真 开始 时 小 球 会 因为 虚拟 环境 中 的 物理 引擎 而 自由 落体 。 

V-REP 的 动态 模块 目前 支持 四 种 不 同 的 物理 引擎 : Bullet 物理 库 ,Open Dynamics 引 
擎 ,Vortex Dynamics 引擎 和 Newton Dynamics 引擎 。 在 任何 时 候 , 用 户 可 以 根据 其 仿真 需 
要 自由 地 快速 从 一 个 发 动机 切换 到 另 一 个 。 物 理 引擎 支持 中 的 这 种 多 样 性 的 原因 是 物理 仿 
真是 一 个 复杂 的 任务 ,可 以 通过 各 种 精度 .速度 或 支持 的 各 种 特征 来 实现 。 
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Bullet physics library: 一 种 开源 物理 引擎 ,具有 3D 碰撞 检测 ,刚体 动力 学 和 软体 动力 
学 (V-REP 目前 不 支持 的 功能 )。 它 用 于 游戏 和 电影 中 的 视觉 效果 ,通常 被 认为 是 一 个 游戏 
物理 引擎 。 

Open Dynamics Engine CODE) : 一 个 开源 物理 引擎 ,具有 两 个 主要 组 件 : 刚体 动力 学 和 
碰撞 检测 。 它 已 经 在 许多 应 用 程序 和 游戏 中 使 用 ,通常 被 认为 是 一 个 游戏 物理 引擎 。 

Vortex Dynamics: 一 个 闭 源 的 ,商业 物理 引擎 产生 高 保 真 物理 仿真 。Vortex 为 大 量 物 
理 属 性 提供 真实 世界 参数 ( 即 对 应 于 物理 单位 ) ,使 得 这 个 引擎 既 实际 又 精确 。Vortex 主要 
用 于 高 性 能 /精密 工业 和 研究 应 用 。V-REP 的 默认 Vortex 插件 是 一 种 商业 产品 ,其 免费 版 
本 目前 支持 在 20s 内 进行 仿真 。 

Newton Dynamics; 它 是 一 个 跨 平台 的 生命 型 物理 仿真 库 。 它 实现 了 一 个 确定 性 求解 
器 , 它 不 是 基于 传统 的 LCP 或 迭代 方法 ,而 是 分 别 具 有 两 者 的 稳定 性 和 速度 。 这 个 功能 使 
Newton 动力 学 不 仅 可 以 用 于 游戏 ,也 可 以 用 于 任何 实时 物理 仿真 。 当 前 插件 实现 是 
BETA 版 本 。 

动态 模块 允许 仿真 接近 真实 世界 的 对 象 之 间 的 相互 作用 。 它 允许 物体 跌落 、 碰 撞 、 反 
弹 , 但 它 也 允许 操纵 者 抓 住 物体 ,传送 带 驱动 部 件 向 前 运动 ,或 车 辆 在 不 平坦 的 地 形 以 现实 
的 方式 滚动 。 

与 许多 其 他 仿真 软件 包 不 同 ,V-REP 不 是 纯 动态 仿真 器 。 它 可 以 被 视 为 一 个 混合 仿真 
器 ,结合 运动 学 和 动力 学 以 获得 各 种 最 佳 性 能 的 仿真 场景 。 现 在 ,物理 引擎 仍然 采用 许多 近 
似 , 并 且 相 对 不 精确 和 缓慢 , 它 尽 可 能 地 尝试 使 用 运动 学 (例如 ,对 于 机 器 人 操纵 器 ) 并 且 仅 
依赖 于 没有 它 则 不 可 行 的 动力 学 (例如 机 器 人 操纵 器 的 夹子 )。 如 果 你 仿真 的 移动 机 器 人 不 
支持 与 其 环境 (大 多 数 移动 机 器 人 反正 很 少 应 该 做 的 ) 碰 撞 或 物理 交互 ,并 且 只 能 在 平地 上 
操作 (其 中 绝 大 多 数 移动 机 器 人 分 组 ) ,那么 可 以 尝试 使 用 运动 或 几何 计算 来 仿真 机 器 人 的 
运动 ,结果 将 更 快 ,也 更 准确 。 动 态 模 块 的 一 些 结果 可 以 通过 图 形 对 象 来 记录 。 


3.5.6 路 径 规 划 模 块 


路 径 规划 模块 通过 一 个 源 自 快速 扩展 随机 树 算法 (RRT) 的 方法 ,允许 完整 的 路 径 规划 
任务 和 非 完 整 的 路 径 规划 任务 ,包括 了 运动 链 的 路 径 规划 。 

V-REP 通过 包装 OMPL 库 的 插件 提供 路 径 / 运 动 规划 功能 。 在 准备 路 径 / 运 动 规划 任 
务 时 应 考虑 以 下 几 点 : 

COD 确定 开始 和 目标 状态 。 当 路 径 规划 对 象 是 串 行 操纵 器 时 ,通常 提供 目标 姿势 (或 未 
端 执行 器 位 置 /取向 ) 而 不 是 目标 状态 。 在 这 种 情况 下 ,函数 simGetConfigForTipPose 可 用 
于 查找 满足 所 提供 的 目标 姿态 的 一 个 或 多 个 目标 状态 。 

(2) 使 用 simExtOMPL createTask 创建 路 径 规划 任务 。 

(3) 使 用 simExtOMPL_setAlgorithm 选择 算法 。 

(4) 创建 所 需 的 状态 空间 , 它 可 以 组 成 一 个 复合 对 象 : simExtOMPL_createStateSpace 
和 simExtOMPL_setStateSpace。 指 定 不 允许 与 simExtOMPL setCollisionPairs 发 生 冲 突 
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的 实体 。 

(5) 使 用 simExtOMPL setStartState 和 simExtOMPL_setGoalState 指定 开始 和 目标 
状态 。 使 用 simExtOMPL compute 计算 一 个 或 多 个 路 径 。 使 用 simExtOMPL_destroyTask £f 
毁 路 径 计 划 任 务 。 通 常 ,路 径 规 划 与 逆 运 动 学 结合 使 用 : 例如 ,在 拾取 和 放置 任务 中 ,最 终 
方法 通常 应 为 直线 路 径 , 可 以 使 用 simGenerateIkPath 生成 。 

以 上 程序 是 常规 方法 ,有 时 缺乏 灵活 性 。 

此 外 ,可 以 设置 以 下 回调 函数 : 


SimExtOMPL setStateValidationCallback 
SimExtOMPL setProjectionEvaluationCallback 
SimExtOMPL setGoalCallback 

SimExtOMPL compute 


提供 的 路 径 通 常 只 是 无 限 可 能 的 路 径 中 的 一 个 路 径 ,并 且 不 能 保证 返回 的 路 径 是 最 优 
解 。 由 于 这 个 原因 ,通常 计算 几 个 不 同 的 路 径 ,然后 选择 更 好 的 路 径 (例如 较 短 的 路 径 ) 。 以 
类 似 的 方式 ,如 果 目 标 状态 必须 从 目标 姿势 计算 , 则 通常 测试 若干 目标 状态 ,因为 不 是 所 有 
目标 状态 都 可 达到 的 或 可 以 足够 接近 的 (在 状态 空间 距离 方面 )。 通 常 的 做 法 是 首先 找到 几 
个 目标 状态 ,然后 根据 它们 到 开始 状态 的 状态 空间 距离 对 它们 进行 排序 。 然 后 ,执行 到 最 近 
的 目标 状态 ,然后 再 到 下 一 个 最 接近 的 目标 状态 。 以 此 类 推 , 直 到 找到 满意 的 路 径 。 


3.6 V-REP 中 控制 机 器 人 仿真 的 方法 


当 完 成 机 器 人 模型 及 环境 的 搭建 后 ,需要 对 机 器 人 进行 控制 ,以 达到 所 需要 的 仿真 效 
果 。V-REP 是 一 个 高 度 可 定制 的 仿真 软件 : 仿真 的 每 个 方面 都 可 以 定制 。 此 外 ,仿真 软件 
本 身 可 以 被 定制 和 定制 以 便 根 据 需 要 正确 地 执行 。 这 是 通过 一 个 复杂 的 应 用 程序 编程 接口 
(APD 完 成 的 。 支 持 六 种 不 同 的 编程 或 编码 方法 ,每 种 编程 或 编码 方法 相对 于 其 他 具有 特 
定 的 优点 (并 且 显 然 也 是 有 缺点 的 ) ,但 是 所 有 六 个 方法 都 是 相互 兼容 的 ( 即 可 以 同时 使 用 ， 
或 者 甚至 并 用 )。 模 型 ,场景 或 仿真 软件 本 身 的 控制 实体 可 以 位 于 : 

覆 入 式 脚本 ( 即 通 过 脚本 定制 仿真 ( 即 一 个 场景 或 多 个 模型 )) : 该 方法 包括 编写 Lua 脚 
本 ,非常 容易 和 灵活 ,保证 与 每 个 其 他 默认 V-REP 安装 的 兼容 性 (只 要 定制 的 Lua 命令 不 
使 用 ,或 者 与 分 布 式 插件 一 起 使 用 )。 该 方法 允许 定制 特定 的 仿真 和 仿真 场景 ,并 且 在 一 定 
程度 上 定制 仿真 器 本 身 。 这 是 最 简单 和 最 常用 的 编程 方法 。 

一 个 附件 : 这 个 方法 ,包括 写 Lua 脚本 ,允许 快速 自 定义 仿真 软件 本 身 。 加 载 项 可 以 自 
动 启动 并 在 后 台 运 行 , 或 者 它们 可 以 被 称 为 函数 (例如 , 当 写 入 进口 商 / 出 口 商 时 很 方便 )。 
附加 组 件 不 应 该 特定 于 某 个 仿真 或 模型 ,它们 应 该 提供 更 通用 的 仿真 软件 绑 定 功能 。 

一 个 插件 ( 即 定制 仿真 软件 和 /或 通过 插件 的 仿真 ) : 这 个 方法 基本 上 在 为 V-REP 编写 
一 个 插件 。 通 常 ,插件 仅 用 于 提供 具有 定制 的 Lua 命令 的 仿真 ,因此 可 与 第 一 种 方法 结合 
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使 用 。 其 他 时 候 ,插件 用 于 为 V-REP 提供 特殊 功能 ,如 需要 的 快速 计算 能 力 ( 脚 本 大 多 数 
时 间 比 编译 语言 慢 ) ,硬件 设备 (例如 真实 机 器 人 ) 的 特定 接口 或 与 外 界 特 殊 的 通信 接口 。 
远程 API 客户 端 ( 即 ,定制 仿真 软件 和 /或 经 由 远程 API 客户 端 应 用 的 仿真 ): 该 方法 


允许 外 部 应 用 (例如 位 了 


F 机 器 人 、 另 一 台 机 器 等 情况 ) 以 非常 连接 的 方式 方便 地 连接 到 


V-REP ,使 用 远程 API 命令 。 这 使 得 可 以 把 运用 在 机 器 人 上 完全 相同 的 代码 调用 来 控制 其 


在 V-REP 内 的 虚拟 挂件 。 


ROS 节点 ( 即 ,定制 仿真 软件 和 /或 经 由 ROS 节点 的 仿真 ) : 该 方法 允许 外 部 应 用 (例如 
位 于 机 器 人 、 另 一 机 器 等 情况 ) 经 由 ROS 连接 到 V-REP。 

定制 客户 端 /服务 器 ( 即 ,经 由 充当 客户 端 或 服务 器 的 外 部 应 用 来 定制 仿真 器 和 /或 仿 
真 ): 客户 端 或 服务 器 应 用 程序 需要 使 用 自 定 义 通信 手段 (例如 套 接 字 ,管道 等 ) 与 脚本 或 插 
件 协 同 工 作 。 这 种 做 法 是 灵活 的 ,但 对 比 以 上 4 种 其 他 方法 ,这 种 方法 更 为 复杂 。 图 3-76 


说 明了 V-REP 中 及 其 周围 的 各 种 定制 可 能 性 。 


V-REP( 开 源 ， 共 享 的 库 ) 


( 


主要 的 客户 端 
应 用 程序 
(可 定制 ) 


C/C++ API 


| i 
远程 API 插 件 ROs 接 品 
(可 定制 的 ) (可 定制 的 ) 


插件 
(定制 的 ) 


[7 
ij] 
GNE 


e 


(8) 
定制 的 
服务 端 /客户 端 


ROS 节 点 
(定制 的 ) 


E] 3-76 V-REP 框架 


如 图 3-76 所 示 ,简要 描述 图 中 所 示 的 各 种 通信 或 消息 传递 机 制 如 下 : 
CD 从 主客 户 端 应 用 程序 或 插件 到 常规 API 的 C/C++ API 调用 。 如 果 语 言 提供 了 一 


276 4| 机 器 人 仿真 与 编程 技术 


种 调用 C 函数 的 机 制 (例如 ,在 Java 的 情况 下 .请 参考 Java 本 机 接口 (JND), 可 以 源 自 非 
C/C++ 应 用 程序 。 

(2) 级 联 子 脚本 执行 。 在 主 脚本 中 使 用 simHandleChildScript 启动 。 

(3) Lua API 从 主 脚本 、 子 脚本 、 回 调 脚 本 或 定制 脚本 调用 到 常规 API。 所 有 调用 都 指 
向 V-REP 引擎 ,除了 调用 插件 的 自 定义 Lua 函数 (参见 下 一 项 ) 。 

(4) 从 仿真 软件 到 插件 的 回调 调用 。 回 调调 用 来 自 Lua 脚本 调用 自 定义 Lua 函数 ( 见 
上 一 项 ) 。 

(5) 事件 回调 从 仿真 软件 到 插件 。 

(6) 来 自 外 部 应 用 程序 ,机 器 人 ,远程 PC 等 的 远程 API 调用 。 

(7) V-REP( 即 其 RosPlugin 或 RosInterface) 与 外 部 应 用 程序 ,机 器 人 ,远程 PC 等 之 间 
的 ROS 数据 交换 。 

(8) f Beg Bol , 串 行 端口 等 连接 到 /从 外 部 应 用 程序 。 

(9) Lua API 调用 从 一 个 附件 到 常规 API。 所 有 调用 都 指向 V-REP 引擎 ,除了 调用 插 
件 的 自 定 义 Lua 函数 。 

(10) 从 V-REP 引擎 回调 调用 到 回调 脚本 。 

(11) 从 V-REP 引擎 执行 调用 到 自 定义 脚本 。 


3.6.1 KARTHIK 


通过 写 一 个 子 脚本 (child script) 去 控制 一 个 给 定 的 机 器 人 或 模型 。 子 脚本 直接 与 场景 
对 象 (scene object) 关 联 , 可 通过 自 定 义 Lua 函数 或 外 部 Lua 函数 库 扩展 。 

嵌入 式 子 脚本 的 优点 是 它 本 身 是 应 用 主 程序 的 一 部 分 ,同时 执行 程序 的 时 候 没 有 通信 
延迟 。 它 的 缺点 是 无 法 选择 其 他 编程 语言 ; 不 能 选择 更 快 的 代码 ; 除了 Lua 函数 库 ,无 法 
直接 访问 外 部 其 他 函数 库 。 

V-REP 是 一 个 高 度 可 定制 的 仿真 软件 : 几乎 仿真 的 每 一 步 都 是 用 户 定义 的 。 集 成 的 
脚本 解释 器 使 V-REP 能 够 实现 这 种 灵活 性 。 脚 本 语言 是 Lua, 它 是 一 种 旨 在 支持 通用 过 程 
编程 的 扩展 编程 语言 。 默 认 情 况 下 .V-REP 使 用 官方 和 原始 的 Lua, 但 是 你 也 可 以 通过 将 
system/ usrset, txt 中 的 变量 useExternalLuaLibrary 设置 为 true 来 告诉 V-REP 使 用 另 一 
种 Lua 风格 。 在 这 种 情况 下 ,所 有 Lua 调用 都 通过 v repLua 库 处 理 , 它 本 身 将 连 杆 到 
LuaJIT(Lua 即时 编译 器 ) v repLua 库 项 目 文件 位 于 / v_repLuaLibrary 中 。 

V-REP 扩展 了 Lua 的 命令 ,并 增加 了 V-REP 特定 命令 ,这 些 命令 可 以 通过 它们 的 sim 
前 缀 (例如 simHandleCollision) 来 识别 。 新 定制 的 Lua 命令 也 可 以 从 主客 户 端 应 用 程序 或 
从 插件 注册 。Lua 的 功能 本 身 可 以 通过 使 用 在 线 可 用 的 Lua 扩展 库 轻 松 扩展 。 典 入 脚本 是 
嵌入 在 场景 (或 模型 ) 中 的 脚本 , 即 作 为 场景 的 一 部 分 并 且 将 与 场景 (或 模型 ) 的 其 余部 分 一 
起 保存 和 加 载 的 脚本 。 

支持 两 种 主要 类 型 的 嵌入 脚本 。 


第 3 章 ”V-REP 在 机 器 人 仿真 中 的 应 用 e 277 


1. 仿真 脚本 

仿真 脚本 是 仅 在 仿真 期 间 执行 的 脚本 ,用 于 自 定义 仿真 或 仿真 模型 。 主 要 仿真 循环 通 
过 主 脚本 处 理 , 模 型 /机 器 人 通过 子 脚 本 控制 ,低级 动态 电机 控制 器 通过 联合 控制 回调 脚本 
编写 ,动态 磁 撞 可 以 通过 联系 回调 脚本 处 理 。 一 般 回 调 脚 本 是 多 用 途 回 调 脚 本 。 

2. 其 他 嵌入 式 脚本 

那些 是 在 仿真 不 运行 时 也 可 以 执行 的 脚本 ,并 且 用 于 定制 仿真 场景 或 仿真 器 本 身 。 它 
们 包括 自 定义 脚本 和 一 般 回调 脚本 。 

V-REP 还 支持 附加 组 件 , 允 许 自 定义 仿真 软件 本 身 。 

在 不 同 的 脚本 类 型 中 , 记 住 一 些 与 场景 对 象 (附加 到 场景 对 象 , 即 相关 联 的 脚本 ) 相 关联 
是 有 用 的 ,例如 子 脚本 、 联 合 控制 回调 脚本 和 定制 脚本 。 相 关 的 脚本 是 形成 V-REP 的 分 布 
式 控 制 架 构 的 基础 ,具有 共享 方便 的 属性 ,如 果 它 们 相关 联 的 对 象 被 复制 , 则 被 自动 复制 。 


3.6.2 插件 


通过 写 一 个 插件 (plugin) 去 控制 一 个 给 定 的 机 器 人 或 模型 。 插 件 允许 回调 机 制 , 自 定 
X. Lua 函数 寄存 和 外 部 函数 库 。 插 件 经 常 被 用 于 与 一 个 子 脚本 相关 联 ,并 相互 结合 。 与 嵌 
入 式 脚本 的 优点 相 类 似 , 插 件 的 优点 是 它 没 有 通信 延迟 ; 并 且 是 应 用 主 程序 的 一 部 分 。 它 
的 缺点 是 编程 复杂 ,并 且 需 要 外 部 编译 。 

插件 是 共享 库 ( 例 如 dll) , 它 在 程序 启动 时 由 V-REP 的 主客 户 端 应 用 程序 自动 加 载 , 或 
者 通过 simLoadModule/simUnloadModule 动态 加 载 / 外 载 。 它 允许 V-REP 的 功能 通过 用 
户 编写 的 功能 扩展 (与 加 载 项 类 似 )。 语 言 可 以 是 能 够 生成 共享 库 并 且 能 够 调用 导出 的 C 
函数 的 任何 语言 (例如 ,在 Java 的 情况 下 .参考 GCJ 和 IKVM)。 插件 也 可 以 用 作 运 行使 用 
其 他 语言 编写 的 代码 ,或 者 甚至 为 其 他 微 控制 器 编写 的 代码 (例如 ,为 Atmel 微 控 制 器 处 理 
和 执行 代码 的 插件 )。 

插件 通常 用 于 定制 仿真 软件 和 /或 特定 的 仿真 。 通 常 ,插件 仅 用 于 为 使 用 自 定义 脚本 的 
命令 提供 仿真 ,因此 与 脚本 结合 使 用 。 其 他 时 候 , 插 件 用 于 为 V-REP 提供 需要 快速 计算 能 
力 ( 脚 本 大 多 数 时 间 比 编译 语言 慢 ) 或 硬件 设备 (例如 实际 机 器 人 ) 的 接口 的 特殊 功能 。 

每 个 插件 需要 有 以 下 3 个 入 口 点 过 程 : 

extern "C"  declspec(dllexport) unsigned char v repStart(void * reserved, int reservedInt); 

extern "C"  declspec(dllexport) void v repEnd(); 


extern "C"  declspec(dllexport) void * v repMessage( int message, int * auxiliaryData, void * 
customData, int * replyData); 


AR dde — A 3E . E AC ERRE. FRENA EXE — T ULA n 
的 目的 : 

1. v_repStart 

这 个 过 程 将 在 主客 户 端 应 用 程序 加 载 插件 后 调用 一 次 。 程 序 应 该 : 
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CD 检查 V-REP 的 版 本 是 否 与 用 于 开发 插件 的 版 本 相同 或 更 高 (只 需 确保 支持 在 插件 
中 使 用 的 所 有 命令 )。 

(2) 如 果 需 要 ,分 配 内 存 , 并 准备 GUI 相关 的 初始 化 工作 。 

(3) 如 果 需 要 ,注册 自 定义 脚本 函数 。 

(4) 如 果 需 要 ,注册 自 定义 脚本 变量 。 

(5) 如 果 初 始 化 成 功 , 则 返回 此 插件 的 版 本 号 ,否则 返回 0。 如 果 返 回 0, 则 印 载 该 插件 
并 且 不 会 操作 。 

2. v_repEnd 

此 过 程 将 在 仿真 循环 退出 之 前 调用 一 次 。 该 过 程 应 该 释放 自 调用 v_repStart 后 保留 的 
所 有 资源 。 

3. v repMessage 

此 过 程 将 在 仿真 软件 运行 时 非常 频繁 地 调用 。 该 程序 负责 监视 感 兴趣 的 消息 并 对 其 作 
出 反应 。 重 要 的 是 对 以 下 事件 做 出 反应 (最 好 通过 拦截 sim_message_eventcallback_ 
instancepass 消息 ) ,具体 取决 于 插件 的 任务 : 

(1) 当 对 象 被 创建 ,销毁 ,缩放 或 加 载 模型 时 : 确保 反映 插件 中 的 更 改 (即将 插件 与 场 
景 内 容 同步 )。 

D 当 加 载 场景 或 调用 undo/redo 功能 时 : 确保 删除 并 重建 连 杆 到 场景 内 容 的 所 有 插 
件 对 象 。 

G) 当场 景 切换 时 : 确保 用 户 删除 并 重建 所 有 与 场景 内 容 连 杆 的 插件 对 象 。 除 此 之 
外 ,请 记 住 ,场景 切换 将 放弃 以 下 项 目的 句柄 : 通信 管 , 信 号 ,横幅 和 绘图 对 象 等 。 

OD 当 仿 真 软件 处 于 编辑 模式 时 : 确保 禁用 插件 提供 的 任何 “特殊 功能 ”, 直 到 编辑 模 
式 结 束 。 特 别 是 ,确保 用 户 不 以 编程 方式 选择 场景 对 象 。 

CO 当 启 动 仿真 时 : 确保 在 需要 时 初始 化 插件 元 素 。 

(6) 当 仿 真 结束 时 : 确保 释放 仿真 期 间 需 要 的 任何 内 存 和 插件 元 素 。 

(7) 当 对 象 选择 状态 更 改 或 发 送 了 对 话 框 刷新 消息 时 : 确保 实现 插件 显示 的 对 话 框 。 

当 编 写 插件 时 ,必须 注意 或 考虑 以 下 几 点 : 

CD 插件 必须 与 主客 户 端 应 用 程序 放 在 同一 目录 中 ,并 遵守 以 下 命名 规则 :“v_ 
repExtXXXX. dll" (Windows) ."libv repExtXXXX. dylib” (Mac OSX) “libv_repExtXXXX. 
so" (Linux) ,其 中 “XXXX” 是 插件 的 名 称 。 使 用 至 少 4 个 字符 ,不 要 使 用 下 划 线 (除了 第 二 
个 位 置 明显 ) ,因为 插件 将 被 忽略 。 

(2) 当 你 的 插件 本 身 加 载 一 些 额 外 的 库 ( 例 如 语言 资源 ,如 “v_repExtXXXX_de. dll". 
等 ))。 注 册 自 定义 脚本 函数 或 脚本 变量 时 .请 尝试 使 用 前 级 .并 为 模块 注册 的 所 有 函数 和 变 
量 (例如 “simExtLab_testMemory”,“simExt_lab_error_value” 等 ) 添 加 前 级 。 避 人 免 使 用 与 
V-REP 的 函数 (“sim”) 相 同 的 前 级 。 建 议 用 户 使 用 “simExtXXXX_” 前 级 。 

G) 在 插件 中 创建 的 线程 应 该 非常 仔细 地 使 用 ,不 应 该 调用 任何 仿真 软件 命令 (用 于 后 
台 计 算 或 与 硬件 通信 ) 。 
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可 以 自由 地 编译 你 的 插件 与 任何 你 想 要 的 编译 器 。 然 而 ,如 果 想 编写 一 个 Qt 插件 ( 即 
使 用 Qt 框架 的 插件 ) ,应 该 注意 : 

COD 需要 编译 具有 与 用 于 编译 V-REP 完全 相同 的 Qt 版 本 的 插件 。 

(2) 应 该 使 用 与 V-REP(Windows 上 为 MinGW.Mac OSX ffl Linux 上 为 GCC) 相同 的 
编译 器 编译 插件 。 

programming/v_repExtPluginSkeleton: 表示 一 个 插件 模板 ,用 户 可 以 使 用 它 创建 自己 
的 插件 。 

programming/v_repExtVision: 处 理 特定 视觉 任务 的 插件 (例如 Velodyne 传感器 的 仿 
真 , 或 全 向 相机 的 仿真 )。 

programming/v repExtSimpleFilter; 说 明 如 何 为 视觉 传感器 编写 自 定义 的 图 像 处 理 
过 滤器 。 插 件 是 一 个 Qt 项 目 。 

programming/v_repExtBubbleRob: 说 明 如 何 添加 定制 的 Lua 函数 以 及 如 何 处 理 几 个 

programming/v_repExtK3: 与 Kheperalll 模型 相关 的 插件 。 

programming/v_repExtRemoteApi: 与 远程 API 相关 的 插件 功能 (服务 器 端 ) 。 

programming/ros_packages/v_repExtRosInterface: ROS 包 ,人 允许 用 户 为 V-REP 构建 
RosInterface。 

programming/ros_packages/vrep _plugin: ROS 包 , 人 允许 用 户 构建 用 于 V-REP 的 
RosPlugin。 

programming/v_repExtMtb: 演示 了 一 个 Qt 插件 , 它 将 机 器 人 语言 解释 器 (或 其 他 仿 
真 软件 ) 集 成 到 V-REP 中 。 


3.6.3 附加 组 件 


附加 组 件 与 插件 十 分 类 似 , 它 在 程序 开始 运行 时 将 会 被 自动 加 载 ,并 且 人 允许 通过 用 户 自 
己 写 的 函数 或 函数 库 扩充 V-REP 的 函数 库 。 附 加 组 件 的 脚本 是 通过 Lua 代码 编程 的 。 支 
持 两 种 类 型 的 附加 组 件 : 

CD 附加 功能 : 附加 功能 首先 出 现在 附加 菜单 中 。 它 们 可 以 被 看 作 是 当 用 户 选择 时 将 
被 执行 一 次 的 函数 。 进 口 商 和 出 口 商 可 以 方便 地 与 它们 一 起 实施 。 

C2) 附加 脚本 : 附加 脚本 在 仿真 软件 运行 时 不 断 执 行 , 有 效 地 在 后 台 运行 。 它 们 应 该 
只 在 每 次 被 调用 时 执行 简约 代码 ,否则 整个 应 用 程序 的 运行 速度 将 会 减 慢 。 根 据 仿真 状态 ， 
附加 脚本 的 调用 方式 不 同 : 

(D 当 仿真 未 运行 ( 即 停止 或 暂停 ) 时 : 在 场景 可 视 化 之 前 调用 附加 脚本 ; 

© 当 仿真 运行 时 : 在 调用 主 脚 本 之 后 调用 附加 脚本 。 

附加 功能 应 该 写 在 与 主 应 用 程序 位 于 同一 文件 夹 中 的 文本 文件 中 ,具有 以 下 命名 约定 : 

(D vrepAddOnFunc. xxxx. lua, HEP xxxx 可 以 是 表示 附加 功能 名 称 的 任何 字符 串 。 

© vrepAddOnScript_xxxx. lua, 其 中 xxxx 可 以 是 表示 附加 脚本 名 称 的 任何 字符 串 。 
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附加 脚本 将 自动 启动 。 在 附加 菜单 中 选择 它 允 许 通过 暂停 /取消 来 暂停 其 执行 。 脚 本 可 以 
通过 返回 0 来 请 求 终止。 

(3) vrepAddOnScript-xxxx. lua. 其 中 xxxx 可 以 是 表示 附加 脚本 名 称 的 任何 字符 串 。 
附加 脚本 不 会 自动 启动 。 用 户 可 以 在 需要 时 通过 在 附加 菜单 中 选择 它 来 开始 /暂停 /取消 暂 
停 。 脚 本 可 以 通过 返回 0 来 请 求 终止 。 

注意 : 不 遵循 以 上 命名 约定 的 附加 脚本 仍然 可 以 通过 命令 行 选 项 加 载 和 运行 。 

只 要 文档 中 没有 另行 说 明 ,加 载 项 可 以 调用 任何 常规 API 函数 。 它 们 甚至 可 以 调用 由 
插件 注册 的 自 定义 Lua 函数 。 然 而 ,它们 有 两 个 限制 ; 

(1) 附加 组 件 不 能 调用 需要 调用 者 在 线程 中 运行 的 API 函数 。 这 是 因为 附加 组 件 以 非 
线程 方式 操作 。 

(2) 虽然 仿真 没有 运行 ,但 附加 组 件 不 应 该 调用 仅 当 运 行 仿真 时 才 有 意义 的 API 
EES 

3.6.4 远程 客户 端 应 用 程序 接口 


通过 外 部 客户 端 应 用 程序 ,可 在 外 部 应 用 程序 、 外 部 的 机 器 人 或 男 一 台 计 算 机 运行 控制 
代码 ; 允许 使 用 运行 于 实际 的 机 器 人 的 代码 控制 模型 或 仿真 。 这 种 方法 的 机 理 是 ,依赖 于 
远程 应 用 程序 接口 的 插件 (在 服务 端 ) 和 远程 应 用 程序 接口 的 代码 (在 用 户 端 )。 它 的 特点 
是 : 程序 与 编程 是 开源 的 ,支持 C/C++ ,Python Java, MATLAB, Octave, Lua 和 Urbi, 

V-REP 是 一 个 函数 库 : 没有 主客 户 端 应 用 程序 (或 主 应 用 程序 或 主 循环 ),V-REP 无 法 
运行 。 安 装 包 附 带 的 默认 主客 户 端 应 用 程序 是 “vrep. exe”(Windows) 或 “vrep”(MacOSX 
和 Linux)。 请 注意 ,在 MacOSX 下 ,客户 端 应 用 程序 以 及 其 他 一 些 项 目 ( 例 如 库 ) 包 含 在 软 
件 包 *vrep. app/Contents/MacOSVvrep” 中 。 

主客 户 端 应 用 程序 是 一 个 小 型 可 执行 文件 ,用 于 处 理 以 下 主要 任务 : 

(1) 它 使 用 simRunSimulator 运行 仿真 软件 ; 

(2) 它 使 用 simLoadModule 和 simUnloadModule Jil Z& £1 1 2X i NF; 

(3) 它 加 载 用 simLoadScene. simLoadModel 或 simLoadUI 双击 的 场景 ,模型 或 UI 
文件 

(4) 它 使 用 simHandleMainScript 和 simAdvanceSimulationByOneStep 处 理 运行 仿真 。 

V-REP 编程 文件 夹 包含 两 个 主要 应 用 程序 项 目 : v_repClientApplication( 跨 平台 Qt 
项 目 ) 和 windowsOnlyProjects/v_repClientApplication(MSV2005 项 目 )。 以 下 是 编译 和 运 
行 应 用 程序 所 需 的 主要 文件 (最 简单 的 是 将 新 建 的 主 应 用 程序 复制 到 V-REP PRO/V-REP 
PRO EDU/V-REP PLAYER 安装 文件 夹 中 ): 

(1)“v_repLib. h"*v repLib. cpp” 和 “v_repConst. h”: 动态 加 载 和 绑 定 到 V-REP 所 需 
的 文件 。 

(2) “v_rep. dlI"/"libv rep. dylib"/^libv rep. so"; V-REP J£. 

(3) “lua5. 1. dll”( 或 类 似 ): Lua 功能 所 需 的 库 。 
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(4) “gscintilla2. dll”( 或 类 似 ) : scintilla 编辑 器 所 需 的 库 。 

(5) “QtCore4. dll" *QtGui4. dll”* 等 (或 类 似 ): Qt 框架 库 。 

(6) V-REP 的 “系统 "文件 夹 及 其 所 有 内 容 ( 正 确 初始 化 等 所 需 )。 

(7)“v_repWin. dll" CX BR Windows): Windows 版 本 V-REP 所 需 的 辅助 库 。 

虽然 主客 户 端 应 用 程序 可 以 定制 ,但 是 不 建议 这 样 做 ,因此 只 有 在 编写 脚本 和 /或 插件 
不 能 用 于 用 户 的 目的 时 才能 使 用 ,因为 如 果 没 有 正确 实现 ,与 默认 V-REP 行为 失去 兼容 性 
的 风险 很 高 。 


3.6.5 通过 ROS 的 节点 


与 远程 API 相 类 似 , 它 具有 几 个 相互 通信 的 分 布 式 流程 ,是 一 种 分 布 式 的 操作 系统 。 
它 能 够 使 得 连接 在 同一 个 网 络 中 的 多 台 计算 机 更 简单 地 被 管理 与 通信 。 在 ROS 的 工作 机 
制 中 ,V-REP 作为 一 个 ROS 节点 ,能 够 通过 3 种 方式 与 其 他 ROS 节点 进行 通信 : V-REP 
节点 提供 ROS 服务 ; V-REP 节点 作为 ROS 发 布 者 ; V-REP 节点 作为 ROS 用 户 。 

有 几 个 ROS 接口 可 用 于 V-REP。 每 一 个 都 提供 一 个 特定 的 行为 .特征 或 一 种 操作 
VET 

(D) RosInterface; 这 是 为 V-REP 开发 的 最 后 一 个 ROS 插件 。 与 RosPlugin 不 同 ， 
RosInterface 以 良好 的 保 真 度 复制 了 C/C++ ROS API。 这 使 其 成 为 通过 ROS 进行 非常 灵 
活 通信 的 理想 选择 ,但 可 能 需要 更 多 地 了 解 各 种 消息 和 ROS 操作 方式 。 

(2) RosPlugin; 这 是 为 V-REP 开发 的 第 一 个 ROS 插件 。 与 RosInterface 不 同 ， 
RosPlugin 不 会 复制 C/C++ ROS API。 相 反 , 它 表示 更 高 级 别 的 抽象 ,应 用 订阅 的 消息 并 
自动 在 场景 对 象 上 发 布 主题 。 这 可 能 会 有 点 混乱 ,但 允许 在 几 种 情况 下 简化 与 ROS 的 交 
互 。 另 一 方面 ,RosPlugin 不 是 很 灵活 ,并 且 不 直接 支持 许多 标准 ROS 消息 。 因 此 ,使 用 
RosInterface 而 不 是 RosPlugin 通常 更 有 意义 。 

(3) ROS skeleton 插件 : 这 表示 一 个 开源 的 skeleton 项 目 .可 用 于 为 V-REP 创建 一 个 
新 的 ROS 插件 。 

(4) 由 其 他 人 开发 的 ROS 接口 : 这 些 不 是 V-REP 直接 支持 的 。 例 如 ,ROS V-Rep 
Bridge。 

所 有 ROS 接口 都 可 以 正常 工作 ,但 强烈 建议 用 户 首先 尝试 RosInterface, 因 为 这 是 最 
灵活 和 自然 的 方法 。 上 面 列 出 的 前 三 个 ROS 接口 的 包 位 于 编程 /ros_packages 中 。 建 议 使 
用 catkin 工具 来 构建 这 些 包 ,否则 可 能 会 遇 到 困难 。 


3.6.6 自 定 义 解决 方案 

自 定义 的 解决 方案 是 通过 不 同 的 方式 (管道 . 串 行 窗 口 套 接 字 ) 与 V- REP 的 插件 或 脚 
本 通信 。 它 的 优点 是 可 选择 不 同 的 编程 语言 ; 可 在 实际 的 机 器 人 或 另 一 台 计算 机 运行 。 它 
的 缺点 是 本 身 的 工作 机 理 和 编程 复杂 ,同时 运行 元 长 。 
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3.7 V-REP 的 API 框架 


V-REPAPI 框架 对 V-REP 周围 的 所 有 接口 进行 分 组 。 它 有 几 种 主要 的 不 同类 型 ， 
。 常规 API; 
远程 API; 
ROS 接口 ; 

。 辅助 API; 

。 其 他 接口 。 

不 仅 可 以 从 仿真 器 内 (例如 从 嵌入 脚本 、 附 加 组 件 、 插 件 或 主客 户 端 应 用 ) 访 问 常规 
API, 也 可 以 从 几乎 任何 可 能 的 外 部 应 用 或 硬件 访问 远程 API 和 ROS 接口 (包括 真实 机 器 
人 ,远程 计算 机 等 )。 辅 助 API 不 是 一 个 接口 本 身 , 而 更 多 的 是 一 个 辅助 函数 的 集合 ,可 以 
嵌入 它们 ,并 且 自 己 操作 。 其 他 接口 项 目 为 用 户 扩展 可 用 接口 的 所 有 可 能 性 。 如 图 3-77 所 
示 , 它 说 明了 V-REP 中 各 种 API 与 其 他 程序 的 连接 。 


x 


远程 API t Tos H 


3-77 V-REP 的 API fic ^g 


3.7.1 常规 API 


常规 API 是 V-REPAPI 框架 的 一 部 分 。 常 规 API 由 几 百 个 可 以 从 C/C++ 应 用 程序 
(插件 或 主客 户 端 应 用 程序 ) 或 退 入 脚本 调用 的 函数 组 成 。 可 以 从 它们 的 “sim” 或 _sim”- 
prefix( 例 如 simHandleCollision) 中 轻松 地 识别 V-REP 函数 和 常量 。 确 保 不 要 混淆 常规 
API( 有 时 也 简称 为 *API”) 与 远程 API 

常规 API 可 以 通过 定制 lua 函数 , 即 插 件 或 主客 户 端 应 用 程序 寄存 器 来 扩展 。 自 定义 
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lua 函数 可 以 从 它们 的 simExt”-prefix 中 识别 。 

V-REP 的 主要 功能 由 以 下 类 型 的 调用 或 命令 处 理 : simHandleXXX 或 simResetXXX 
(例如 simHandleCollision, simResetCollision 等 )。 命令 期 望 的 常规 参数 是 通用 类 型 对 象 
(例如 ,碰撞 对 象 的 句柄 ,距离 对 象 的 句柄 等 ) 的 句柄 。 想 象 一 下 ,你 通过 碰撞 检测 对 话 框 注 
册 了 一 个 碰撞 对 象 "robotCollision”。 如 果 你 现在 想 检 查 该 对 象 的 碰撞 状态 ,你 可 以 写 下面 
的 代码 : 

collisionObjectHandle = simGetCollisionHandle("robotCollision")collisionState = simHandleCollision 

(collisionObjectHandle) 


第 一 行 检索 名 为 “robotCollision ”的 碰撞 对 象 的 句柄 。 第 二 行 显 式 地 处 理 碰 撞 对 象 
( 即 , 它 对 该 特定 碰撞 对 象 执行 碰撞 检测 )。 在 这 种 情况 下 ,碰撞 对 象 必 须 标记 为 显 式 处 理 ， 
否则 生成 错误 。 

上 面 两 个 相同 的 命令 也 可 以 以 非 显 式 方式 处 理 对 象 : 而 不 是 使 用 碰撞 对 象 句柄 作为 参 
数 ,也 可 以 使 用 sim_handle_all 参数 ; 


simHandleCollision(sim handle all) 


sim, handle all 参数 允许 一 次 处 理 所 有 已 注册 的 碰撞 对 象 。 这 里 ,以 非 显 式 方式 处 理 
碰撞 对 象 。 

当 注 册 通 用 类 型 的 对 象 (例如 碰撞 对 象 ) 时 ,你 不 需要 任何 特定 的 代码 行 来 处 理 对 象 , 因 
为 主 脚本 正在 处 理 ( 注 册 一 个 新 的 碰撞 对 象 通过 指定 两 个 实体 ,应 该 检查 碰撞 ,运行 仿真 并 
将 两 个 实体 中 的 一 个 移动 到 另 一 个 上 : 将 检测 到 碰撞 ,并 且 两 个 实体 将 以 不 同 的 颜色 出 现 
以 指示 碰撞 )。 事 实 上 , 主 脚 本 包含 以 下 默认 代码 : 


simHandleCollision(sim handle all except explicit)simHandleDistance(sim handle all except 
explicit)simHandleProximitySensors(sim handle all except explicit) 


命令 的 参数 是 sim handle all except explicit, CHA 5j sim. handle. all 相同 的 效果 ， 
具有 以 下 异常 : 标记 为 显 式 处 理 的 对 象 将 不 会 被 处 理 。 大 多 数 通用 类 型 的 对 象 可 以 被 标记 
为 显 式 处 理 。 当 被 标记 为 非 显 式 处 理 时 ,只 有 当 使 用 对 象 句柄 (例如 simHandleCollision 
(collisionObjectHandle)) 或 者 使 用 sim. handle. all 参数 (例如 simHandleCollision (sim | 
handle alD ) 调 用 命令 时 ,它们 才 会 被 处 理 。 

默认 情况 下 , 显 式 处 理 标志 被 禁用 ,这 意味 着 默认 通用 类 型 对 象 由 主 脚本 处 理 。 然 而 ， 
如 果子 脚本 希望 (或 需要 ) 自 己 处 理 对 象 . 则 应 该 启用 显 式 处 理 标志 ,和 否则 生成 错误 。 通 常 ， 
只 有 当 子 脚本 在 同一 个 仿真 过 程 中 需要 多 次 计算 新 的 计算 结果 时 (否则 子 脚本 可 以 使 用 诸 
如 simReadCollision.simReadDistance. simReadProximitySensor 等 命令 ) ,才能 明确 地 处 理 
对 象 。 另 一 方面 , 子 脚本 不 应 该 使 用 sim. handle all 或 sim handle all except explicit 参 
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数 。 如 图 3-78 和 图 3-79 所 示 , 它 显示 了 显 式 和 非 显 式 调用 。 


碰撞 对 象 1 
(检查 对 象 2,3 之 间 的 碰撞 ) 


场景 对 象 。 场景 对 象 


不 要 标记 为 “explicit handling” 


碰撞 对 象 


命令 只 读 取 “碰撞 对 象 1” 的 结 9 


SmHangieComsion(Sm_nandie_all_except_expbcn) ] 


EWE 命令 还 可 以 处 理 “ 碰 撞 对 象 1" 
图 3-78  CollisionObject1 通过 默认 主 脚本 ( 非 显 式 处 理 ) 处 理 ( 即 计算 ), 并 且 结果 在 子 脚本 中 读 取 


碰撞 对 象 


场景 对 象 场景 对 象 


CollOb/! HandlessimGetCollisionHandie( ColisionObject" ) 
colisionCheckResuttssimReadCollision|collOb;! Handie) 


命令 处 理 “ 碰 撞 对 象 1” 


SimHandieColision(sim handie all except explicit) 


主 脚本 命令 无 法 处 理 “ 碰 摘 对 象 1” 


图 3-79 CollisionObjectl 由 子 脚 本 处 理 ( 即 计算 )( 显 式 处 理 ) 


3.7.2 远程 API 


V-REP 提供 远程 API. 允许 从 外 部 应 用 或 远程 硬件 (例如 真实 机 器 人 、 远 程 计算 机 等 ) 
控制 仿真 (或 仿真 器 本 身 )。V-REP 远程 API 由 大 约 一 百 个 特定 函数 和 一 个 通用 函数 组 
成 ,可 以 从 C/C++ 应 用 程序 、Python 脚本 Java 应 用 程序 、MATLAB/Octave 程序 、Urbi 脚 
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本 或 Lua 脚本 进行 程序 (代码 ) 的 编写 。 远 程 API 函数 通过 套 接 字 通信 与 V-REP 进行 交 
互 ,在 很 大 程度 上 减少 了 滞后 和 网 络 负载 。 所 有 这 些 都 以 隐藏 的 方式 发 生 给 用 户 。 远 程 
API 可 以 允许 一 个 或 多 个 外 部 应 用 程序 以 同步 或 异步 方式 (默认 为 异步 ) 与 V-REP 进行 交 
互 , 甚 至 支持 仿真 器 的 远程 控制 (例如 远程 加 载 场景 ,开始 、 和 暂停 或 停止 仿真 场景 ) 。 

在 每 个 仿真 通过 与 远程 API 应 用 同步 地 运行 ( 即 ,仿真 器 将 在 :十 dz 时 间 等 待 来 自 客 户 
端的 触发 信号 开始 下 一 个 仿真 通过 ) 的 意义 上 使 用 。 这 与 阻塞 / 非 阻 塞 操作 的 意义 上 的 同 
步 /异步 不 同 。 远 程 API 还 支持 阻塞 和 非 阻塞 操作 。 

远程 API 功能 分 为 两 个 独立 的 实体 ,通过 套 接 字 通信 进行 交互 : 

CD 客户 端 ( 即 应 用 程序 ) : 客户 端 上 的 远程 API 可 用 于 许多 不 同 的 编程 语言 。 目 前 支 
持 以 下 语言 : C/C++ ,Python ,Java,MATLAB,Octave,Urbi 和 Lua。 其 他 语言 的 绑 定 可 以 
很 容易 地 自己 创建 。 

(2) 服务 器 端 ( 即 V-REP): 服务 器 端的 远程 API 通过 V-REP 插件 实现 ,应 该 由 
V-REP 默认 加 载 : v_repExtRemoteApi. dll, libv_ repExtRemoteApi. dylib 或 libv_ re- 
pExtRemoteApi. so。 插 件 项 目 文件 位 于 V-REP 的 安装 日 录 中 的 “编程 "文件 夹 中 。 

1. 启用 远程 API 的 客户 端 

D C/C++ 客户 端 

要 在 C/C++ 应 用 程序 中 使 用 远程 API 功能 ,只 需 在 项 目 中 包含 以 下 C 语言 文件 : 


extApi.h 
extApi.c 
extApiPlatform. h( 包 含 平台 特定 代码 ) 
extApiPlatform.c( 包 含 平台 特定 代码 ) 


以 上 文件 位 于 V-REP 的 安装 目录 下 ,在 编程 /remote API F. 确保 已 将 NON. 
MATLAB PARSING 和 MAX_EXT_API_CONNECTIONS= 255 定义 为 预 处 理 器 定义 。 
要 在 客户 端 ( 即 应 用 程序 ) 上 启用 远程 API, 请 调用 simxStart。 此 页 面 列 出 并 描述 所 有 支持 
的 C/C++ 远程 API 函数 。V-REP 远程 API 函数 可 以 从 其 “simx”-prefix 中 轻松 识别 。 

2) Python 客户 端 

要 在 Python 脚本 中 使 用 远程 API 功能 ,需要 以 下 3 个 项 目 : 


vrep. py 
vrepConst.py 
remoteApi. dll, remoteApi.dylib 或 remoteApi. so( 取 决 于 目标 平台 ) 


以 上 文件 位 于 V-REP 的 安装 目录 下 ,在 编程 /remoteApiBindings/python 下 。 可 能 必 
须 自 己 构建 remoteApi 共享 库 ( 使 用 remoteApiSharedLib. veproj 或 makefile) (如 果 尚 未 构 
建 )。 在 这 种 情况 下 ,请 确保 已 将 NON MATLAB PARSING fl MAX_EXT_API_ 
CONNECTIONS 二 255 定义 为 预 处 理 器 定义 。 
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在 Python 中 已 知 的 目录 中 有 以 上 元 素 后 ,调用 importvrep 以 加 载 库 。 要 在 客户 端 ( 即 
的 应 用 程序 ) 上 启用 远程 API, 请 调用 vrep. simxStart。 此 页 面 列 出 并 描述 所 有 支持 的 
Python 远程 API 函数 。V-REP 远程 API 函数 可 以 从 其 “simx”-prefix 中 轻松 识别 。 
3) Java 客户 端 
要 在 Java 应 用 程序 中 使 用 远程 API 功能 ,需要 以 下 2 个 项 目 : 


coppelia( 包 含 12 个 Java 类 ) 
renoteApiJava. dll, libremoteApiJava.dylib 或 libremoteApiJava. so( 取 决 于 目标 平台 ) 


以 上 文件 位 于 V-REP 的 安装 目录 ,编程 /remoteApiBindings/java 下 。 可 能 必须 自己 
构建 remoteApijava 共享 库 ( 使 用 remoteApiSharedLibJava. veproj 或 remoteApiSharedLibJava_ 
Makefile)( 如 果 尚 未 构建 )。 在 这 种 情况 下 ,请 确保 已 将 NON_MATLAB_PARSING 和 
MAX_EXT_API_CONNECTIONS=255 定义 为 预 处 理 器 定义 。 

在 Java 中 已 知 的 目录 中 具有 上 述 元 素 后 ,使 用 javaemyA ppName. java 编译 具有 
myAppName. java 的 应 用 程序 。 在 应 用 程序 中 ,确保 导入 与 importcoppelia. className 一 
起 使 用 的 类 ,然后 调用 remoteApivrep 王 newremoteApi() 加 载 库 。 要 在 客户 端 ( 即 的 应 用 程 
序 ) 上 启用 远程 API, 请 调用 vrep. simxStart。 本 页 列 出 并 描述 了 所 有 支持 的 Java 远程 API 
函数 。V-REP 远程 API 函数 可 以 从 其 “simx”-prefix 中 轻松 识别 。 

可 能 还 需要 将 文件 夹 添加 到 系统 路 径 。 例 如 ,在 Linux 中 ,可 以 在 执行 Java 应 用 程序 
之 前 调用 : exportLD_LIBRARY_PATH = $ LD_LIBRARY_PATH; 'pwd'。 

4) MATLAB 客户 端 

要 在 MATLAB 程序 中 使 用 远程 API 功能 .需要 以 下 3 个 项 目 : 


remoteApiProto.m 
remApi.m 
remoteApi.dll,remoteApi.dylib sk remoteApi. so( 取 决 于 目标 平台 ) 


以 上 文件 位 于 V-REP 的 安装 目录 下 .在 编程 /remoteApiBindings/matlab 下 。 可 能 必 
须 自己 构建 remoteApi 共享 库 ( 使 用 remoteApiSharedLib. veproj 或 remoteApiSharedLib_ 
Makefile)( 如 果 尚 未 构建 ) 。 

一 旦 在 MATLAB 的 当前 文件 夹 中 有 以 上 元 素 , 调 用 vrep 二 remApi('remoteApi') 来 构 
建 对 象 并 加 载 库 , 要 在 客户 端 ( 即 的 应 用 程序 ) 上 启用 远程 API, 请 调用 vrep. simxStart。 请 
参见 编程 /remoteApiBindings/matlab 目录 中 的 simpleTest. m 程序 的 示例 。 本 页 列 出 并 描 
述 了 所 有 支持 的 MATLAB 远程 API 函数 。V-REP 远程 API 函数 可 以 从 其 "simx”-prefix 
中 轻松 识别 。 

确保 MATLAB 使 用 与 remoteApi 库 相 同 的 位 体系 结构 : 64 位 MATLAB 与 32 位 
remoteApi 库 将 不 工作 ,反之 亦 然 。 

如 果 不 得 不 重建 remoteApi 库 , 可 能 需要 重新 生成 原型 文件 (remoteApiProto. m); f$ 
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先 ,确保 有 一 个 MATLAB 识别 的 编译 器 。 可 能 需要 调用 mex-setup。 然 后 , 输入 
loadlibrary C'remoteApi'. 'extApi. h', 'mfilename', 'remoteApiProto'), 

5) Urbi 

要 在 Octave 程序 中 使 用 远程 API 功能 ,需要 以 下 2 个 项 目 : 


remApiSetup.m 
remApi.oct 


以 上 文件 位 于 V-REP 的 安装 目录 中 ,在 编程 /remoteApiBindings/urbi 下 。 可 能 必须 
自己 构建 remoteApiUrbi 共享 库 ( 如 果 尚 未 构建 )。 在 这 种 情况 下 ,请 确保 已 将 NON | 
MATLAB PARSING 和 MAX EXT API. CONNECTIONS - 255 定义 为 预 处 理 器 定义 。 
在 Windows 下 ,使 用 remoteApiSharedLibUrbi. veproje {E Linux/Mac F ,执行 以 下 步 又， 

(D 在 Urbi 根 目录 中 创建 一 个 extApiUrbi. uob 文件 夹 。 

O 将 以 下 元 素 复 制 到 该 新 文件 夹 中 : extApi. c, extApi. h, extApilnternal. h, 
extApiPlatform. c,extApiPlatform. h.v. repConst. h 和 extApiUrbi. cpp。 

@ 从 上 面 的 文件 夹 中 ,输入 : 


. . /bin/umake - q-- shared - library - oremoteApiUrbi. soEXTRA CPPFLAGS = — DNON MATLAB PARSING 
— DMAX EXT API CONNECTIONS = 255 


要 从 脚本 加 载 远 程 API 功能 ,请 调用 loadFileC" remoteApiConst. urbi") fil loodModule 
("remoteApiUrbi")。 要 在 客户 端 ( 即 应 用 程序 ) 上 启用 远程 API, 请 调用 vrep. simxStart。 
此 页 面 列 出 并 描述 所 有 支持 的 Urbi 远程 API 函数 。V-REP 远程 API 函数 可 以 从 其 
"simx"-prefix 中 轻松 识别 。 

2. 启用 远程 API 的 服务 器 端 

远程 API 服务 器 端 通过 基于 常规 API 的 V-REP 插件 实现 。 远 程 API 插件 项 目 位 于 
V-REP 的 安装 目录 "programming/v_repExtRemoteApi" 下 。 

要 在 服务 器 端 启用 远程 API( 即 在 V-REP 端 ), 请 确保 在 V-REP 启动 (v_re- 
pExtRemoteApi. dll,libv_repExtRemoteApi. dylib 或 libv repExtRemoteApi. so) 中 成 功 加 
载 了 远程 API 插件 (可 以 检查 控制 台 窗 口 有 关 插 件 加 载 的 信息 )。 远 程 API 插件 可 以 根据 
需要 启动 多 个 服务 器 服务 (每 个 服务 将 在 不 同 的 端口 上 侦 听 /通信 )。 服 务 器 服务 可 以 以 两 
种 不 同 的 方式 启动 : 

(1) 在 V-REP 启动 (连续 远程 API 服务 器 服务 )。 远 程 API 插件 将 尝试 读 取 名 为 
remoteApiConnections. txt 的 配置 文件 ,并 根据 其 内 容 启动 适当 的 服务 器 服务 。 使 用 此 方 
法 远程 控制 仿真 器 本 身 。 使 用 这 种 方法 ,远程 API 函数 将 总 是 在 服务 器 端 执行 ,即使 仿真 
没有 运行 (这 在 下 一 个 方法 中 并 不 总 是 这 样 )。 还 有 男 一 种 方法 可 以 通过 命令 行 启动 连续 的 
远程 API 服务 器 服务 。 

(2) 从 脚本 (临时 远程 API 服务 器 服务 ) 中 启动 .大 多 数 时 候 这 是 启动 远程 API 服务 器 
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服务 的 首选 方法 。 用 户 在 服务 启动 或 停止 时 处 于 控制 之 下 。 但 是 , 当 从 仿真 脚本 启动 临时 
远程 API 服务 器 服务 时 ,服务 将 在 仿真 结束 时 自动 停止 。 可 以 使 用 以 下 2 个 自 定义 Lua K 
数 (插件 导出 2 个 函数 ) 来 启动 或 停止 临时 远程 APT 服务 器 服务 : 


simExtRemoteApiStart 
simExtRemoteApiStop 


可 以 使 用 以 下 自 定义 Lua 函数 收集 有 关 任 何 远 程 API 服务 器 服务 的 信息 (该 函数 由 插 
件 导出 ): 


simExtRemoteApiStatus 


可 以 使 用 以 下 自 定 义 Lua 函数 重 置 ( 即 销毁 和 重新 创建 ) 任 何 远 程 API 服务 器 服务 (该 
函数 由 插件 导出 ) : 


simExtRemoteApiReset 


3. ix f2 API 操作 方式 

远程 API 函数 以 类 似 于 常规 API 函数 的 方式 调用 ,但 是 有 两 个 主要 区 别 : 

CD 大 多 数 远程 API 函数 返回 类 似 的 值 : 返回 代码 。 始 终 记 住 返回 码 是 位 编码 的 ( 因 
此 必须 测试 各 个 位 ,以 便 正确 解释 它 ) 。 

(2) 大 多 数 远程 API 函数 需要 两 个 附加 参数 : 操作 模式 和 clientID( 由 simxStart 函数 
返回 的 标识 符 ) 。 

对 操作 模式 和 特定 返回 代码 的 需要 来 自 于 远程 API 函数 必须 通过 套 接 字 通 信 到 服务 
器 (V-REP) ,执行 任务 ,然后 返回 到 调用 者 (客户 机 ) 的 事实 。 一 个 朴素 的 (或 常规 的 ) 方 法 
是 让 客户 端 发 送 请 求 ,并 等 待 ,直到 服务 器 处 理 请 求 并 回复 。 在 大 多 数 情况 下 ,这 将 花费 太 
多 的 时 间 ,滞后 将 会 影响 客户 端 应 用 程序 。 相 反 ,远程 API 允许 用 户 通过 提供 4 个 主要 机 
制 来 执行 函数 调用 或 控制 仿真 进度 ,来 选择 操作 模式 的 类 型 和 仿真 进展 的 方式 : 

。 阻塞 函数 调用 ; 

。 非 阻塞 函数 调用 ; 

。 数据 流 ; 

。 同步 操作 。 

1) 阻塞 函数 调用 

阻塞 函数 调用 是 一 种 朴素 或 正常 的 方法 ,意味 着 我 们 不 能 等 待 服务 器 的 回复 的 情况 ,如 
下 面 的 情况 : 

// 以 下 函数 (阻塞 模式 ) 将 检索 对 象 句柄 : 


if(simxGetObjectHandle (clientID," myJoint", &jointHandle, simx opmode blocking) == simx_ 
return ok) 
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{ 
// 这 里 我 们 有 联合 句柄 在 变量 jointHandle 
H 


如 图 3-80 所 示 , 它 说 明了 阻塞 函数 调用 : 
通过 函数 块 调用 从 V-REP 中 读 取 数据 


V-REP 仿 真 远程 API 服 务 器 进程 i 远程 API 客 户 端 进程 远程 API 客 户 端 (如 MATLAB) 
I 
处 理 远程 API 服 务 器 | 有 
| 
EIER 
( ) 1 
ttdt( 仿 真 时 间 ) | 执行 延迟 
re | 
data | 
I 


| 


运行 主 脚本 


! data 
! 
CERE) | T CE 
p—————Ü L 一 一 一 : 
处 理 远程 API 服 务 器 | 可 用 数据 
运行 主 脚 本 


图 3-80 阻塞 函数 调用 


2) 非 阻塞 函数 调用 
非 阻 塞 函 数 调用 适用 于 当 我 们 只 想 要 向 V-REP 发 送 数据 而 不 需要 回复 的 情况 ,如 下 
面 的 情况 : 


// 跟 随 功能 ( 非 阻塞 模式 ) 将 设置 关节 的 位 置 : 
simxSetJointPosition(clientID, jointHandle, jointPosition,simx opmode oneshot); 


如 图 3-81 所 示 , 它 说 明了 非 阻 塞 函 数 调用 。 

在 一 些 情况 下 ,重要 的 是 能 够 在 同一 消息 内 发 送 各 种 数据 ,以 便 在 服务 器 侧 同时 应 用 该 
数据 (例如 , 想 要 将 机 器 人 的 3 个 关节 应 用 于 其 V-REP 模型 在 同一 时 间 , 即 在 相同 的 仿真 步 
又 )。 在 这 种 情况 下 ,用 户 可 以 暂时 停止 通信 线程 以 实现 这 一 点 ,如 以 下 示例 所 示 : 


SimxPauseCommunication(clientID,1) ;simxSetJointPosition(clientID, jointlHandle, jointlValue, 
simx opmode oneshot); simxSetJointPosition(clientID, joint2Handle, joint2Value, simx opmode | 
oneshot); simxSetJointPosition (clientID, joint3Handle, joint3Value, simx opmode _ oneshot ); 
simxPauseCommunication(clientID, 0); 

// 上 面 的 3 个 关节 将 被 同时 接收 和 设置 在 V-REP 侧 
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通过 非 模块 函数 调用 发 送 数 据 到 V-REP 
V-REP 仿 真 远程 API 服 务 器 进程 。| 远程 API 客 户 端 进程 远程 API 客 户 端 (MATLAB) 
E | i men 
1 1  Opmode:simx opmode oneshot 
1 Return code: 
运行 主 脚本 t f 
1 ectPosition 
{=ttdt( 仿 真 时 间 ) 1 Opmode: simx opmode oneshot 
1 | Mtm: 
E E a 
i 
运行 主 脚本 | 
ttrdt( 仿 真 时 间 ) ! 
1 
1 
i 
运行 主 脚本 i 


图 3-81 JEBILSE eR H 


如 图 3-82 所 示 , 它 说 明了 临时 停止 通信 线程 的 影响 。 
在 相同 的 仿真 步 又 中 将 命令 /数据 应 用 于 V-REP 


V-REP 仿 真 远程 API 服 务 器 线程 | 远程 API 客 户 端 线程 远程 API 客 户 端 (例如 ,MATLAB) 
| 一 一 二 
( 即 ， 一 个 仿真 步 ) | | M ES 
GE) | — sinxsetobjectPosition 
t=ttdt( 仿 真 时 间 ) | 8 
| ü =r simxPauseCommunication: off 
区 行 主 脚本 1 
( 即 一 个 仿真 步 ) 1 
Gite) | 
{ttdt( 仿 真 时 间 ) | 
A ' 
运行 主 县 本 ' 
图 3-82 ”临时 暂停 通信 线程 
3) 数据 流 


服务 器 可 以 预期 客户 端 需要 什么 类 型 的 数据 。 为 了 发 生 这 种 情况 ,客户 端 必须 以 “ 流 ” 
或 “连续 ”操作 模式 标志 向 服务 器 发 信号 通知 该 期 望 ( 即 ,功能 存储 在 服务 器 侧 , 在 常规 时 间 
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基础 上 执行 和 发 送 ,而 不 需要 来 自 客 户 端的 请 求 )。 这 可 以 看 作 是 从 客户 端 到 服务 器 的 命 
令 / 消 息 订阅 ,其 中 服务 器 将 流 式 传 输 数 据 到 客户 端 。 这 样 的 流 操作 请 求 和 流 数据 的 读 取 在 
客户 端 看 起 来 像 这 样 。 
如 图 3-83 所 示 , 它 说 明了 数据 流 的 原理 。 
通过 数据 流 读 取 来 自 V-REP 中 的 数据 


V-REP 仿 真 远程 API 服 务 器 线 程 。 | 远程 AP 客户 员 线 得 ERAP PAM. Matlab) 
处 理 远程 API 服 务 器 1 一 
1 ! — Opmode:simx opmode streaming. 
运行 主 脚本 Return code- sims. retur. novalue flag 
( 即 ， 一 个 仿真 步 ) | 
qii) i 4 | 
thdt( 仿 真 时 间 ) 1 EN eee 
~ ! Return code: six, ratum. novalue. fag 
运行 主 脚本 ow ! 设 有 执行 延 时 
( 即 ， 一 个 仿真 步 ) | "he | 
( 泻 染 ) 1 二 一 sinxoetJointPosition g 
Opmode: timx opmode buffer 
人 ttdt( 仿 真 时 间 ) 1 em Return code: sims. retum ok. : 
Ae 1 1 
处 理 远程 API 服 务 器 —— | == x E 
运行 主 脚本 mac 
(QU. 一 个 仿真 步 ) SS | Return code: sim». return. ok 
Git) | 二 一 simxgatjointPoaition 
T Opmode:simx opmode buffer 
ttdt( 仿 真 时 间 ) ! 数据 Retur code: sim. retur. ok : 
处 理 远程 API 服 务 器 1 
E 
图 3-83 数据 流 


一 旦 完成 数据 流传 输 , 远 程 API 客户 端 应 始终 通知 服务 器 ( 即 V-REP) 停 止 流传 输 该 数 
据 , 和 否则 服务 器 将 继续 流传 输 非 必需 数据 并 最 终 减 慢 速度 。 使 用 simx_opmode_discontinue 
操作 模式 。 

4) 同步 操作 

从 上 面 的 函数 调用 ,你 可 能 已 经 注意 到 一 个 仿真 将 提前 或 进步 ,而 不 考虑 远程 API 客 
户 端的 进度 。 默 认 情 况 下 ,远程 API 函数 调用 将 异步 执行 。 然 而 ,存在 远程 API 客户 端 需 
要 与 仿真 进程 同步 的 情况 ,通过 控制 来 自 远程 API 客户 端的 仿真 进展 。 这 可 以 通过 使 用 远 
FE API 同步 模式 来 实现 。 在 这 种 情况 下 .远程 API 服务 器 服务 需要 预先 启用 以 进行 同步 操 
作 ( 这 可 以 通过 simExtRemoteApiStart 函数 或 通过 连续 远程 API 服务 器 服务 配置 文件 
remoteApiConnections. txt 实现 ) 。 如 图 3-84 所 示 , 它 说 明了 同步 模式 的 工作 原理 。 

当 调 用 simxSynchronousTrigger 时 ,下 一 个 仿真 步骤 将 开始 计算 。 这 并 不 意味 着 当 函 
数 调用 返回 时 ,下 一 个 仿真 步骤 将 完成 计算 。 因 此 ,必须 确保 读 取 正 确 的 数据 。 如 果 没 有 采 
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同步 模式 
V-REP 仿 真 远程 API 服 务 器 线程 ”| 远程 API 客 户 端 线程 远程 API 客 户 端 (例如 ，Matlaby 


一 simxGetJointPosition 
LLL IHR opmode sims opmode, hockine 
| 
1 
上 
| 


| HN 


1 EN simxSynchronousTrigger 
上 
| 

L 
运行 主 脚本 i 
( 即 ， 一 个 仿真 步 ) ^ 1 EN . 
Gic) | 

i 


图 3-84 同步 模式 


取 特 殊 措 施 , 可 能 会 从 先前 的 仿真 步骤 或 当前 仿真 步骤 读 取 数 据 , 如 图 3-85 所 示 。 

在 客户 端 ,至 少 有 2 个 线程 将 运行 : 主线 程 ( 将 调用 远程 API 函数 的 线程 ) 和 通信 线程 
(将 处 理 数据 传输 的 线程 场景 )。 在 客户 端 ,根据 需要 可 以 有 尽 可 能 多 的 通信 线程 ( 即 通信 线 
路 ) : 确保 为 它们 中 的 每 一 个 调用 simxStart。 使 用 V-REP 插件 实现 的 服务 器 端 以 类 似 的 
方式 操作 。 图 3-86 说 明了 远程 API 的 工作 原理 。 

以 下 描述 了 各 种 支持 的 操作 模式 : 

(1) simx opmode oneshot; 非 阻 塞 模式 。 将 命令 发 送 到 服务 器 以 便 执行 (1)-(b)-(3) 。 
如 果 可 用 (iD-(2), 则 从 本 地 缓冲 器 返回 对 相同 命令 的 答复 ,但 是 先前 执行 的 答复 。 该 函数 
不 等 待 来 自 服务 器 (7)-(iD 的 答复 。 在 服务 器 侧 ,命令 临时 存储 (4)-(d) ,执行 一 次 (d)-(9)- 
(g) 和 回复 (g)-(6)。 此 模式 通常 与 “set-functions”( 例 如 simxSetJointPosition) 一 起 使 用 ， 
其 中 用 户 不 关心 返回 值 。 

(2) simx_opmode_blocking: 阻 寨 模式 。 向 服务 器 发 送 命 令 以 执行 (1)-(b)-(3), 并 上 且 
该 功能 等 待 来 自 服务 器 (7)-(i)-(2) 的 应 答 。 然 后 ,将 从 收 件 箱 缓冲 器 (i) 中 擦 除 所 接收 的 应 
答 ,其 不 会 与 其 他 操作 模式 一 起 发 生 。 在 服务 器 侧 ,命令 临时 存储 (4)-(d) ,执行 一 次 (d)- 
(9)-(g) 和 回复 (g)-(6)。 此 模式 通常 与 “get-functions”( 例 如 simxGetObjectHandle) 一 起 使 
用 ,其 中 用 户 需要 对 发 送 的 命令 进行 回复 。 

(3) simx opmode streaming: 非 阻塞 模式 。 将 命令 发 送 到 服务 器 以 执行 (1)-(b)-(3) 。 
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同步 模式 -数据 更 新 (1) 
远程 API 客 户 端 


V-REP 仿 真 远程 API 服 务 器 线程 远程 API 客 户 端 线程 (l; MATLAB) 


1 
| 
1 
1 Opmode: simx_opmode_streaming 
1 
1 
1 
1 


„n 一 simxSynchronousTrigger 
W D 


运行 主 脚本 
(Le. one simulation step) 


(Rendering) 


tat+dt [simulation time] 


图 3-85 ”同步 模式 (数据 更 新 ) 


如 果 可 用 (D-(2), 则 从 本 地 缓冲 器 返回 对 相同 命令 的 答复 ,但 是 先前 执行 的 答复 。 该 函数 
不 等 待 来 自 服务 器 (7)-(i) 的 答复 。 类 似 于 simx_opmode_oneshot, 但 区 别 在 于 命令 将 存储 
在 服务 器 侧 (4)-(e) ,连续 执行 (e)-(9)-(g), 并 连续 发 送 回 客户 端 -(6)。 此 模式 通常 与 “get- 
functions”( 例 如 simxGetJointPosition) 一 起 使 用 ,其 中 用 户 不 断 地 需要 特定 的 值 。 

(4) simx_opmode_oneshot_split( 不 推荐 ): 非 阻塞 模式 。 逐 步 ( 以 小 数据 块 ) 向 服务 器 
发 送 命令 以 执行 (1)-(a)-(3)。 如 果 可 用 (iD-(2), 则 从 本 地 缓冲 器 返回 对 相同 命令 的 答复 ， 
但 是 先前 执行 的 答复 。 该 函数 不 等 待 来 自 服务 器 (7)-(h)-(i) 的 答复 。 当 命令 完全 发 送 时 ， 
它 从 (Ca) 中 删除 。 在 服务 器 侧 , 命 令 块 被 重新 存储 (4)-(c) ,并 且 当 命令 被 完全 接收 时 ,命令 
将 被 执行 一 次 (5)-(d)-(9)-(f) 逐 渐 发 回 客户 端 (f)-(6) 。 客 户 机 以 小 块 (7)-(h) 接 收 答复 ,并 
且 当 答复 完成 时 , 它 被 存储 在 本 地 缓冲 器 (8)-(i) 中 。 该 模式 通常 和 与 大 量 数据 相关 联 的 
“设置 函数 ”( 例 如 simxSetVisionSensorImage) 一 起 使 用 ,以 便 不 使 通信 网 络 过 载 。 

(5) simx_opmode_streaming_split( 不 推荐 ): 非 阻塞 模式 。 逐 步 ( 以 小 数据 块 ) 向 服务 
器 发 送 命令 以 执行 (1)-(a)-(3)。 如 果 可 用 (D-(2), 则 从 本 地 缓冲 器 返回 对 相同 命令 的 答 
复 , 但 是 先前 执行 的 答复 。 该 函数 不 等 待 来 自 服务 器 (7)-(h)-(i) 的 答复 。 当 命令 完全 发 送 
时 , 它 从 (a) 中 删除 。 在 服务 器 侧 , 命 令 块 被 重新 存储 (4)-(c) ,并且 当 命 令 被 完全 接收 时 
(5)-(e) ,命令 将 被 连续 地 执行 (e)-(9) 和 回复 逐渐 发 送 回 客户 端 (f)-(6)。 客 户 机 以 小 块 
(7)-(h) 接 收 应 答 ,并 且 当 应 答 完成 时 . 它 被 存储 在 本 地 缓冲 器 (8)-(i) 中。 该 模式 通常 和 与 
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客户 端 (应 用 程序 ) 
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远程 API 代 码 
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控制 
代码 


非 部 分 cmd 回 
复 的 收 件 箱 


| O 


—Q 


远程 API 
- 国 数 调用 


用 于 部 分 拆 分 
cmds 的 收 件 箱 
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ii 收 件 箱 A 
[| 流 式 命令 的 
收 件 箱 © 


A 4r iflemdial 
复 的 发 件 箱 © 


服务 端 (远程 API 插 件 ) 


图 3-86 远程 API 功能 概述 


大 量 数据 (例如 simxGetVisionSensorImage) 相 关联 的 “获取 函数 ”一 起 使 用 ,其 中 用 户 需 要 
不 断 地 数据 而 不 使 通信 和 网络 过 载 。 

(6) simx_opmode_discontinue: 非 阻塞 模式 。 向 服务 器 (1)-(b)-(3) 发 送 命令 。 如 果 可 
用 (iD-(2), 则 从 本 地 缓冲 器 返回 对 相同 命令 的 答复 ,但 是 先前 执行 的 答复 。 该 函数 不 等 待 
来 自 服务 器 (7)-(iD 的 答复 。 在 服务 器 端 ,命令 简单 地 清除 位 于 (e) 中 的 类 似 命令 。 向 客户 
端 (6)-(7) 发 送 回 复 , 其 也 将 清除 位 于 (D 中 的 类 似 回 复 。 此 模式 用 于 释放 (i) 中 的 一 些 存储 
器 (类 似 于 simx_opmode_remove) 或 中 断 流 命令 ( 即 ,通过 从 (e) 中 去 除 它 们 ) 。 

(7) simx_opmode_buffer: 非 阻塞 模式 。 没 有 命令 被 发 送 到 服务 器 ,但 是 如 果 可 用 (iD- 
(2) , 则 从 本 地 缓冲 器 返回 先前 执行 的 对 同一 命令 的 回复 。 此 模式 通常 与 simx_opmode_ 
streaming 或 simx opmode streaming split 操作 模式 结合 使 用 : 首先 ,使 用 流 式 命令 启动 
常量 命令 执行 ,然后 提取 只 有 命令 的 回应 。 

(8) simx_opmode_remove: 非 阻塞 模式 。 没 有 命令 被 发 送 到 服务 器 ,但 是 只 有 对 先前 
执行 的 相同 命令 的 答复 ,如 果 存 在 (D , 则 从 本 地 缓冲 器 清除 。 该 函数 不 返回 任何 值 , 除 了 返 
回 代码 。 此 模式 可 用 于 释放 客户 端 上 的 一 些 内存 . 但 很 少 需要 。 
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3.7.3 ROS 接口 


前 面 关于 V-REP 的 编程 方法 已 经 介绍 了 ROS 接口 ,下面 将 具体 地 介绍 ROS 接口 以 及 
ROS 插 件 。 

1. Roslnterface 

RosInterface 是 V-REPAPI 框架 的 一 部 分 ,由 FedericoFerri 提供 。 确 保 不 要 混淆 
RosInterface 与 RosPlugin. 它们 是 V-REP 中 的 两 个 不 同 的 ROS 接口 。 与 RosPlugin 不 
同 ,RosInterface 以 良好 的 保 真 度 复制 了 C/C++ ROSAPI。 这 使 其 成 为 通过 ROS 进行 非常 
灵活 通信 的 理想 选择 ,但 可 能 需要 更 多 地 了 解 各 种 消息 和 ROS 操作 方式 。 可 以 从 
simExtRosInterface 前缀 中 识别 RosInterfaceAPI 函数 。ROS 是 一 种 分 布 式 伪 操作 系统 ， 
允许 在 网 络 中 连接 的 多 台 计 算 机 之 间 轻 松 管理 和 通信 。 

V-REP 可 以 作为 ROS 节点 ,其 他 节点 可 以 通过 ROS 服务 .ROS 发 布 者 和 ROS 订阅 者 
进行 通信 。 

V-REP 中 的 RosInterface 功能 通过 插件 启用 : libv_repExtRosInterface. so 或 libv_ 
repExtRosInterface. dylib。 插 件 的 代码 是 开源 的 ,位 于 programming/ros_packages 文件 夹 
中 。 该 插件 可 以 很 容易 地 适应 自己 的 需要 。 插 件 在 V-REP 启动 时 加 载 ,但 加 载 操 作 只 有 
在 roscore 当时 正在 运行 时 才 会 成 功 。 确 保 检查 V-REP 的 控制 台 窗口 或 终端 有 关 插 件 加 
载 操 作 的 详细 信息 。 

V-REP 本 身 提供 了 几 个 与 RosInterface 相关 的 . ttt 场景 文件 。 


rosInterfaceTopicPublisherAndSubscriber. ttt 
controlTypeExamples.ttt 
Models/tools/rosInterfacehelpertool. ttn( 允许 在 同步 模式 下 操作 V- REP 的 模型 ,例如 为 了 手动 步 
进 仿真 ). 
2. RosPlugin 
RosPlugin 是 V-REPAPI 框架 的 一 部 分 。 确 保 不 要 混淆 RosPlugin 与 RosInterface, 这 
是 V-REP 中 的 两 个 不 同 的 ROS 接口 。 与 RosInterface 不 同 , RosPlugin 不 会 复制 C/C++ 
ROSAPI。 相 反 地 , 它 表 示 更 高 级 别 的 抽象 ,应 用 订阅 的 消息 并 自动 在 场景 对 象 上 发 布 主 题 。 
这 可 能 会 有 点 混乱 ,但 允许 在 几 种 情况 下 简化 与 ROS 的 交互 。 另 一 方面 ,RosPlugin 不 是 
很 灵活 ,并 且 不 直接 支持 许多 标准 ROS 消息 。 因 此 ,使 用 RosInterface 而 不 是 RosPlugin 
通常 更 有 意义 。 可 以 从 simExtROS_ 前 级 中 识别 RosPluginAPI 函数 。 
ROS 是 一 种 分 布 式 伪 操 作 系 统 ,允许 在 网 络 中 连接 的 多 台 计算 机 之 间 轻 松 管理 和 通 
信 。V-REP 作为 其 他 节点 可 以 通过 以 下 3 种 方式 进行 通信 的 ROS 节点 : 
(1) V-REPRosPlugin 节点 提供 ROS 服务 。V-REP 只 要 启动 V-REP, 就 可 以 使 用 
RosPlugin 服务 (假定 roscore 正在 运行 , 且 Rosplugin 已 正确 加 载 到 V-REP) 。 
(2) 可 以 启用 V-REPRosPlugin 来 发 布 主题 ,并 向 其 发 布 数据 。V-REPRosPlugin 发 布 
商 只 能 启用 ,并 且 只 能 在 运行 仿真 时 运行 。 一 个 例外 是 info 主题 : 只 要 V-REP 正在 运行 ， 
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该 主题 的 数据 将 被 流 式 传输 。 

G) 可 以 启用 V-REPRosPlugin 以 订阅 主题 ,从 中 读 取 数 据 ,并 将 数据 应 用 于 V-REP 
中 的 特定 对 象 /项 目 。V-REPRosPlugin 用 户 只 能 启用 ,并 且 只 能 在 仿真 运行 时 运行 。 

V-REP 中 的 RosPlugin 功能 通过 以 下 插件 启用 : libv repExtROS. so 或 libv_ 
repExtROS. dylib。 插 件 的 代码 是 开源 的 ,位 于 编程 /ros_packages 文件 夹 中 。 该 插件 可 以 
很 容易 地 适应 自己 的 需要 。 插 件 在 V-REP 启动 时 加 载 ,但 加 载 操作 只 有 在 roscore 当时 正 
在 运行 时 才 会 成 功 。 确 保 检查 V-REP 的 控制 台 窗 口 或 终端 有 关 插 件 加 载 操 作 的 详细 
信息 。 

如 图 3-87 所 示 , 它 说 明了 如 何在 服务 器 端 ( 即 在 V-REPRosPlugin 端 ) 处 理 ROS 消息 。 


publishTopics(); 
ros::spinOnce(); 


Ew 


A, m 


额外 的 ros 插 件 


publishTopics(); 
ros::spinOnce(); 


一 一 ROS 的 消息 
ROS 节 点 (nodes) 


图 3-87 ROS 消息 处 理 ,服务 器 端 
V-REP 本 身 提供 了 几 个 与 RosPlugin 相关 的 . ttt 场景 文件 : 
rosPluginTopicPublisherAndSubscriberl.ttt 


rosPluginTopicPublisherAndSubscriber2.ttt 
controlTypeExamples. ttt 


3.7.4 辅助 API 
辅助 API 不 是 一 个 接口 本 身 , 而 是 更 多 的 辅助 函数 集合 ,可 以 符 入 到 自己 的 代码 中 ,并 
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且 自 己 操作 。 

可 以 使 用 以 下 辅助 API 类 别 : 外 部 运动 学 : 这 是 一 个 C++ 函数 集合 ,提供 与 V-REP 中 
相同 的 运动 学 计算 。 

这 个 函数 集合 允许 执行 的 运动 学 计算 与 在 V-REP 中 相同 。 对 此 的 例外 是 障碍 物 避 免 
功能 , 它 仅 在 V-REP 内 支持 。 

这 个 想法 是 通常 在 V-REP 中 构建 运动 任务 ,然后 导出 场景 的 运动 内 容 , 然 后 可 以 直接 
使 用 下 面 的 嵌入 函数 。 所 需 的 源 代 码 位 于 编程 /externalIk 文件 夹 中 。 确 保 将 所 有 文件 包 
含 在 项 目 中 ,并 在 需要 访问 函数 的 文件 中 包含 extIk. h。 还 要 确保 知道 如 何 使 用 V-REP 中 
的 运动 学 功能 。 如 果 可 以 访问 常规 API, 那 么 将 不 需要 此 辅助 API, 因 为 所 有 以 下 函数 都 有 
其 常规 API 等 效 项 。 

外 部 IK 源 代码 不 直接 是 V-REP 的 一 部 分 ,并 且 带 有 单独 的 许可 条 件 。 

按照 以 下 方法 从 我 们 自己 的 外 部 应 用 程序 中 执行 运动 学 计算 : 

(1) 在 V-REP 中 构建 运动 任务 ,测试 它们 。 

(2) 依次 单 击 菜单 栏 文件 .导出 、IK 内 容 导出 场景 的 运动 内 容 。 

(3) 在 自己 的 应 用 程序 中 包含 外 部 运动 代码 (代码 位 于 编程 /externallk 文件 夹 中 )。 

COD 在 应 用 程序 启动 时 调用 smEmbLaunch, 在 应 用 程序 结束 时 调用 simEmbShutDown。 

(5) 调用 simEmbStart 来 导入 以 前 导出 的 文件 。simEmbStart 可 以 根据 需要 频繁 调用 
以 重 置 运动 场景 。 运 动 场景 类 似 于 V-REP 中 的 场景 ,除了 它 被 去 除了 非 运 动 学 的 一 切 。 

(6) 调用 各 种 函数 来 移动 /旋转 目标 虚拟 变量 (例如 ,使 用 simEmbSetObjectTrans- 
formation) ,或 移动 非 活 动 关节 , 即 不 处 于 IK 模式 的 关节 (例如 ,使 用 simEmbSetJointPosi- 
tion) 。 

(7) 调用 simEmbHandleIkGroup 来 执行 一 次 计算 通过 ( 即 有 效 地 将 虚拟 指针 带 到 它们 
的 目标 上 )。 如 果 我 们 正在 搜索 特定 的 机 器 人 配置 ,或 需要 立即 跳 转 到 新 的 末端 效应 器 姿 
35 ,然后 调用 simEmbGetConfigForTipPose。 

(8) 根据 需要 重复 步骤 7 和 8。 确 保 检查 返回 值 以 检测 错误 。 

(9) 如 果 有 多 个 同一 机 器 人 的 实例 , 则 可 以 调用 simEmbLaunch 几 次 来 初始 化 嵌入 运 
动 学 的 几 个 实例 。 然 后 ,我 们 可 以 使 用 simEmbSwitch 从 一 个 实例 切换 到 另 一 个 实例 。 


3.7.5 其 他 接口 


可 以 以 各 种 方式 扩展 V-REP API 框 架 , 以 便 提 供 不 直 接 是 V-REP 一 部 分 的 接口 。 这 
通常 通过 插件 发 生 . 但 是 也 可 以 使 用 其 他 选项 (如 附加 组 件 . 远 程 API 客户 端 .ROS 节点 
等 )。 以 下 仅 列 出 其 中 几 个 : OMPL 接口 ,基于 Qt 的 自 定义 UI 界面 ,V-REP 5j ROS 之 间 
的 桥架 等 。 
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3.8 仿真 模型 的 搭建 


种 ， 


V-REP 的 模型 及 其 工作 环境 是 由 场景 对 象 组 成 的 。 而 这 些 场景 对 象 的 来 源 主 要 有 两 


一 种 是 V-REP 本 身 所 提供 的 模型 ,另外 一 种 则 需要 利用 外 部 CAD 软件 进行 建 模 , 再 导 


人 到 V-REP 中 。 因 此 ,V-REP 的 模型 搭建 有 3 种 方法 : 


(1) 从 模型 浏览 器 中 加 载 现 有 模型 ， 
(2) 从 菜单 栏 中 添加 基础 形状 ; 
(3) 从 Import 命令 中 加 载 其 他 软件 的 CAD 模型 。 


3.8.1 从 模型 浏览 器 中 加 载 现 有 模型 
在 模型 浏览 器 中 选择 机 器 人 及 其 他 模型 ,然后 用 鼠标 拖 昌 到 场景 中 ,场景 中 将 自动 加 载 


相应 的 机 器 人 及 其 他 模型 ,并 给 予 它们 任意 的 位 姿 。 因 此 ,加 载 后 的 模型 一 般 需要 进行 位 置 


在 工具 栏 中 ,选择 object/item shift 的 按钮 可 在 三 维 空间 中 对 机 器 人 及 其 他 模型 的 位 置 进 


行 设置 ; 选择 object/item rotate 可 对 机 器 人 及 其 他 模型 在 三 维 空间 的 姿态 进行 旋转 操作 。 


3.8.2 从 菜单 栏 中 添加 场景 对 象 
在 场景 中 , 右 击 Add 窗口 可 添加 模型 所 需要 的 元 素 ,包括 基础 形状 、 力 传感器 、 近 距离 


传感器 ,关节 摄像 机 灯光 等 元 素 。 其 中 ,在 基础 形状 中 ,包括 了 平面 、 圆 柱 .长方体 ,球体 和 
圆 简 等 基础 形状 。 将 这 些 元 素 添加 到 场景 中 后 ,都 必须 先 对 其 大 小 .位 置 ,姿态 等 参数 进行 
设置 。 


状 。 


添加 原始 形状 时 ,通过 依次 单 击 菜单 栏 . 添 加、 原始 形状 ,可 以 在 V-REP 中 直接 创建 形 
其 支持 以 下 5 种 基本 形状 : 平面 , 圆 盘 ,长 方 体 .球体 和 圆柱 体 。 

如 图 3-88 所 示 ,基本 形状 的 几何 参数 可 以 在 原始 形状 对 话 框 中 进行 调整 。 

Q) X/Y/Z 尺寸 : 沿 着 世界 参考 框架 的 X/Y/Z 轴 的 尺寸 。 

(2) X-/Y-/Z- 细 分 : 沿 着 世界 参考 框架 的 X-/Y-/Z- 轴 的 元 素数 量 ( x )。 

(3) 侧面 : 圆柱 、 圆 盘 或 球体 的 边 数 。 

(4) 面 数 : 柱 面 或 球面 的 面 数 。 

(5) 光盘 细 分 : 光盘 细 分 数 ( 光 盘 或 光 柱 ) 。 

(6) 平滑 阴影 : 形状 是 否 应 该 看 起 来 光滑 或 锐利 。 

C 开口 端 : 气缸 是 否 有 开口 端 。 

(8) 锥 体 : 表示 需要 锥 体 而 不 是 圆柱 体 。 

(9) 创建 动态 和 可 响应 形状 : 如 果 选 择 ,生成 的 形状 将 是 动态 的 和 可 响应 的 。 它 不 会 


是 可 碰撞 的 ,不 可 测量 的 、 不 可 呈现 的 和 不 可 检测 的 ,并 且 具 有 淡 蓝 色 , 以 便 快 速 识别 它 。 


(10) 创建 纯 形状 : 如 果 选 择 ,将 生成 纯 形 状 ,在 动态 计算 期 间 执行 得 更 好 、 更 快 。 


第 3 章 ”V-REP 在 机 器 人 仿真 中 的 应 用 |P 299 


Primitive cuboid 


Xx-size [m] [Looooeo Xx-subdivisons [o à 
10 


图 3-88 ”原始 形状 对 话 框 


AD 材料 密度 : 材料 的 密度 。 

(12) 形状 具有 负 体 积 : 当选 中 时 ,创建 的 纯 形 状 将 具有 内 部 ( 即 , 负 体积 ) 的 孔 , 其 具有 
由 内 部 缩放 因子 缩放 的 形状 的 尺寸 。 这 是 创建 可 以 动态 有 效仿 真 的 管状 结构 的 有 用 特征 。 
目前 ,只 有 Vortex Dynamics 引擎 支持 此 功能 。 

具有 更 高 细 分 计数 的 形状 可 以 以 更 多 不 同 的 方式 反射 照明 。 颜 色 和 其 他 视觉 属性 应 在 
形状 属性 中 进行 调整 。 另 外 ,也 可 以 在 形状 编辑 模式 下 或 在 几何 对 话 框 中 调整 和 编辑 形状 
网 格 。 


3.8.3 从 Import 命令 中 导 人 /导出 其 他 软件 的 CAD 模型 


构建 新 的 模型 时 需要 导入 所 需要 的 部 件 , 使 用 菜单 栏 File 中 的 Import 命令 ,可 加 载 在 
其 他 三 维 建 模 软件 做 好 的 CAD 模型 。 加 载 的 模型 支持 obj,3ds,dxf,stl,csv 等 格式 。 导 入 
后 的 CAD 模型 将 会 直接 呈现 在 场景 中 ,因此 还 需要 对 模型 的 位 置 .大 小 .外 观 颜色 等 进行 
设置 。 

从 Import 命令 中 导入 其 他 软件 的 CAD 模型 需要 注意 以 下 三 点 : 

(1) V-REP 描述 和 显示 模型 的 形状 时 使 用 了 三 角 网 络 , 这 使 得 模型 的 形状 可 直接 用 于 
动力 学 。 模 型 所 包含 的 三 角形 越 多 ,所 需要 的 计算 时 间 越 长 。 从 外 部 导入 CAD 模型 必须 
考虑 到 模型 的 繁重 程度 , 即 不 能 包含 过 多 的 三 角形 。 

一 般 情 况 下 ,V-REP 中 模型 所 包含 的 三 角形 总 数 不 能 超过 20000, 最 佳 范围 是 5000 一 
10000。 当 模型 太 过 繁重 时 ,需要 对 模型 的 各 部 分 进行 简化 。 

(2) CAD 模型 导入 到 场景 中 时 .会 自动 弹出 一 个 Import Option 窗口 ,让 用 户 选择 导入 
模型 网 络 的 范围 和 姿态 。 其 中 可 选 网 络 范围 有 ”1 unit represents 1 meter" "1 unit 
represents 1 foot"*1 unit represents 10 centimeter” 等 选项 ,姿态 上 提供 了 Y 向 量 朝 上 和 Z 
向 量 向 上 两 个 选择 。 用 户 应 该 按照 具体 的 任务 需求 选择 模型 网 络 的 范围 和 姿态 。 在 导入 至 
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场景 后 ,还 可 以 利用 object/item shift 和 object/item rotate 进行 模型 位 置 和 姿态 调整 。 

(3) CAD 模型 导入 到 场景 中 时 ,只 是 一 个 简单 随机 形状 (Simple random shape)。 例 
如 ,导入 一 个 机 器 手臂 模型 ,整体 上 只 是 一 个 单纯 的 形状 ,底座 、 连 杆 、 末 端 执行 器 等 部 分 都 
没有 分 离开 来 ,不 具有 自由 度 。 因 此 .需要 把 模型 划分 成 分 离 的 部 分 ,这 可 通过 两 种 方式 实 
现 : 自动 网 络 划分 和 手动 网 络 划分 。 第 一 种 方式 是 按照 导入 之 前 CAD 模型 的 网 络 结构 进 
行 划分 的 ; 第 二 种 是 使 用 三 角形 编辑 模式 ,手动 增添 .删除 三 角形 ,对 模型 的 网 络 进行 再 次 
划分 。 如 图 3-89 所 示 , 导 入 的 小 车 模型 为 一 个 单纯 的 形状 ,进行 自动 网 络 划分 后 ,成 为 了 由 
几 个 部 分 组 成 的 形状 组 合 。 


图 3-89 导入 的 模型 与 使 用 自动 网 络 划分 后 的 效果 


1. 导入 形状 

V-REP 使 用 三 角形 网 格 来 描述 和 显示 形状 。 因 此 ,V-REP 只 能 导入 将 对 象 描述 为 三 
角形 网 格 的 格式 。 然 而 ,如 果 希 望 导 入 其 他 描述 的 对 象 ,例如 描述 为 参数 曲面 (如 IGES, 
STEP 等 ) , 则 必须 先 将 文件 转换 为 适当 的 三 角形 网 格格 式 。 有 几 个 转换 应 用 程序 可 以 实现 
此 操作 ,并 且 大 多 数 3D 绘图 应 用 程序 也 支持 这 种 操作 。 

V-REP 支持 以 下 文件 格式 导入 形状 : 

(1) OBJ: Wavefront Technologies 文件 格式 。 这 是 目前 唯一 允许 在 V-REP 中 导入 纹 
理 网 格 的 格式 。 

(2) DXF: AutoCAD 文件 格式 (Autodesk) 。 包 含 在 文件 中 的 非 3D 信息 可 能 被 忽略 。 

(3) 3DS; 3ds Max 文件 格式 (Autodesk)。 纹 理 信息 会 被 忽略 。 此 格式 目前 仅 支持 
Windows 平 台 。 

(4) STLCASCII 或 二 进 制 ): 3D 系统 文件 格式 。 支 持 ASCI 和 二 进 制 文件 。 

(5) COLLADA ; 有 关 详 细 信息 ,请 参阅 collada 插件 。 

(6) URDF: 有 关 详 细 信 息 . 请 参阅 URDF 插件 。 

在 导入 操作 期 间 ,与 纹理 相关 的 缩放 对 话 框 可 能 会 打开 。 与 网 格物 体 相关 的 另 一 个 对 
话 框 也 将 打开 。 

如 图 3-90 所 示 ,导入 选项 对 话 框 允 许 我 们 正确 设置 导入 文件 中 使 用 的 单位 ( 某 些 应 用 
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程序 可 能 使 用 单位 米 , 其 他 的 可 能 使 用 英尺 、 英 寸 、 厘 米 或 
毫米 ) 以 及 网 格 的 方向 ( 某 些 应 用 程序 中 的 Y 轴 指 向 朝 上 ， 


Import Options 
Mesh scaling 


其 他 程序 中 是 Z 轴 朝 上 )。 在 任何 情况 下 ,我 们 都 可 以 在 对 | @ runt 
象 公共 属性 中 缩放 导入 的 形状 ,或 在 方向 操作 模式 中 更 正 | DITE 


O 1unit represents 10 centimeters 


导入 的 形状 。 O 1untrepresents 1inch 


O 1unitrepresents 1 centimeter 


导入 的 形状 将 具有 默认 的 视觉 参数 和 随机 颜色 ,以 区 | O 1urtrepresents i imet 
分 各 个 形状 。 纹 理 只 能 通过 OBI 文件 格式 导入 : 在 这 种 情 | meh ees 
况 下 ,纹理 将 被 加 载 并 应 用 于 导入 的 网 格 。 如 果 纹 理 加 载 | reee 
操作 失败 , 则 只 导入 网 格 和 纹理 坐标 ,并 且 稍 后 可 以 通过 纹 
理 对 话 框 应 用 纹理 。 

在 导入 操作 期 间 ,V-REP 将 确保 网 格 对 象 一 致 ,并 删 
除 未 使 用 的 顶点 ,合并 彼此 靠近 的 顶点 等 。 可 以 在 用 户 设 。“” 图 390 导入 选 项 对 话 框 
置 对 话 框 的 顶点 /三 角形 验证 设置 中 进行 设置 。 如 果 在 导 
入 操作 后 ,我 们 无 法 在 场景 中 看 到 任何 形状 ,但 场景 层次 结构 表明 存在 新 添加 的 形状 , 则 很 
可 能 我 们 导入 的 形状 太 大 或 太 小 ,导致 无 法 看 到 。 另 外 ,可 以 在 对 象 公共 属性 中 继续 缩放 操 
作 。 此 外 , 当 从 CAD 应 用 程序 导出 网 格 时 ,程序 会 尝试 将 它们 作为 整体 导出 (最 好 是 将 它 
们 导出 为 单个 对 象 ,在 V-REP 中 ,可 以 依次 单 击 菜单 栏 、 编 辑 ` 分 组 /合并 、 分 割 选 定 的 形 
状 ); 这 是 为 了 避免 CAD 应 用 程序 在 导出 操作 期 间 根据 它们 的 参考 帧 重新 定位 /重新 定向 
各 个 网 格 (V-REP 的 参考 框架 是 不 同 的 ) ,这 可 能 导致 外 观 的 破坏 。 

确保 导入 的 网 格 不 包含 太 多 的 三 角形 (对 于 机 器 人 ,通常 在 10000 — 20000 个 三 角形 之 
间 ) ,否则 V-REP 可 能 会 大 大 减 慢 ( 由 于 泻 染 、 计 算 、 加 载 /保存 操作 等 )。 有 一 些 应 用 程序 
可 以 减少 网 格 中 的 多 边 形 数 量 ( 例 如 MeshLab)。 还 可 以 使 用 以 下 V-REP 功能 : 

CD 依次 单 击 菜单 栏 编辑、 变形 选择 成 凸 形 : 允许 将 选 定 的 形状 转换 为 凸 形 。 

(2) 依次 单 击 菜单 栏 编辑、 变形 选择 到 其 凸 分 解 : 允许 将 选 定 的 形状 转换 为 它们 的 凸 
分 解 表示 。 

(3) 依次 单 击 菜单 栏 编辑 .抽取 所 选 形状 : 允许 减少 所 选 形状 中 的 三 角形 数量 。 

(4) 依次 单 击 菜单 栏 编辑 .提取 选 定形 状 : 允许 从 相同 形状 的 外 部 提取 /分 离 形 状 ( 即 
不 可 见 部 分 ) 的 内 部 ( 即 可 见 部 分 )。 该 功能 基于 视觉 传感器 ,并 且 可 能 无 法 给 出 令 人 满意 的 
结果 。 

当 从 处 理 形 状 作为 参数 表面 (例如 IGES, STEP 等 ) 的 应 用 程序 中 导出 形状 时 ,如 果 绘 
图 由 不 同 大 小 的 对 象 组 成 , 则 需要 用 多 个 步骤 导出 对 象 ;这 是 为 了 避免 具有 过 于 精确 定义 的 
大 对 象 ( 太 多 的 三 角形 ) 和 太 粗 略 定义 的 小 对 象 之 间 的 精度 混乱 : 首先 导出 大 对 象 (通过 调 
整 所 需 的 精度 设置 ) ,然后 再 导入 小 对 象 (通过 调整 精度 设置 ) 。 

2. 导出 形状 

V-REP 通过 依次 单 击 菜单 栏 , 文 件 . 导 出 \ 所 选 形状 导出 形状 ,其 支持 以 下 的 文件 格式 
(注意 : 只 导出 所 选 对 象 ) : 
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。 DXF: AutoCAD 文 件 格 式 (Autodesk)。 

e OBJ: Wavefront Technologies 文件 格式 。 

。 STL( 二 进 制 ): 3D 系统 文件 格式 。 仅 导出 支持 二 进 制 格式 。 这 是 V-REP 中 最 常用 

的 导出 选项 。 

3. 导入 高 度 字段 

前 面 提 到 ,高 度 形状 可 以 将 地 形 表示 为 规则 网 格 ,其 中 只 有 高 度 变化 。 高 度 场 也 可 以 被 
看 成 是 纯 简单 形状 ,并 且 被 用 于 优化 动态 碰撞 响应 计算 。 

V-REP 支持 以 下 文件 格式 导入 高 度 场 形状 (依次 单 击 菜单 栏 文件 、 导 入、 高 度 ): 

COD 图 像 文件 : 图 像 文 件 (JPEG,PNG,TGA,BMP,TIFF 或 
GIF 文件 ) ,其 中 各 种 高 度 值 取 自 红 、 绿 和 蓝 色 分 量 : height = 


Heightfield Dimensi... 


(red + green 十 blue)/3, PrE m 
(2) CSV s TXT: 逗号 分 隔 值 文件 格式 。 该 文件 应 包含 | 0m om 
行 ,其 中 每 行 具有 用 逗号 分 隔 的 工 值 。 n 


选择 要 导入 的 文件 后 ,将 打开 一 个 对 话 框 ,如 图 3-91 所 示 ， 图 3.91  Heighifield 导入 
对 Heightfield 导入 选项 对 话 框 的 说 明 如 下 : 选项 对 话 框 
(OD XX 尺寸 /Y 尺寸: 指定 高 度 字段 的 X 和 Y 尺寸 。 每 个 高 
度 单元 都 是 方形 。 
(2) Z 缩放: 指定 要 应 用 于 高 度 值 的 缩放 。 


3.9 机 器 人 的 仿真 


对 搭建 好 的 场景 进行 仿真 主要 有 3 个 步骤 : 

O 选择 物理 引擎 ; 

© 设置 仿真 参数 ; 

@ 进行 仿真 。 

可 以 依次 单 击 菜单 栏 ,仿真 .开始 /暂停 /停止 仿真 或 通过 相关 工具 栏 按钮 启动 ,暂停 和 
停止 V-REP 中 的 仿真 。 

脚本 和 程序 应 始终 考虑 当前 的 调用 类 型 和 /或 仿真 状态 ,以 便 正 确 运 行 。 最 好 将 每 个 控 
制 代码 至 少 划分 为 4 个 部 分 (对 于 非 线程 子 脚本 ) : 

。 初始 化 部 分 : 初始 化 部 分 应 该 仅 在 第 一 次 调用 脚本 或 程序 时 运行 。 

。 仿真 部 分 . 致 动 : 该 部 分 是 致 动 实际 发 生 的 地 方 。 

。 仿真 部 分 , 感 测 : 这 部 分 是 感 测 实际 发 生 的 地 方 。 

。 恢复 部 分 : 恢复 部 分 应 该 在 上 次 调用 脚本 或 程序 时 运行 。 

仿真 器 通过 在 恒定 时 间 步 长 推进 仿真 时 间 来 操作 。 如 图 3-92 所 示 , 它 说 明了 主要 的 仿 
真 循环 。 如 图 3-93 所 示 ,可 以 通过 尝试 保持 仿真 时 间 与 实时 同步 来 支持 实时 仿真 。 


第 3 章 “V-REP 在 机 器 人 仿真 中 的 应 用 | 303 


仿真 时 间 
simulation time-0 
运行 主 脚本 


仿真 时 间 =0 


仿真 时 间 = 仿 真 时 间 + 步 长 


图 3-92 主要 仿真 循环 图 3-93 ”实时 仿真 循环 


仿真 时 间 = 仿 真 时 间 + 仿 真 步 长 


以 下 代表 一 个 非常 简化 的 主要 客户 端 应 用 程序 (为 了 表示 清楚 ,消息 ,插件 处 理 和 其 他 
细节 已 被 省 略 ) : 


void initializationCallback 
( 
//do sone initialization here 


) 


void loopCallback 
{ 
if ( (simGetSimulationState( )&sim simulation advancing)!= 0 ) 


{ 
if ( (simGetRealTimeSimulation()!- 1) | | (sinIsRealTimeSimulationStepNeeded() == 1) ) 
t 
if ((simHandleMainScript()&sim script main script not called) == 0) 
simAdvanceSimulationByOneStep(); 
) 
) 
) 
void deinitializationCallback 
t 
//do some clean- up here 
) 


根据 仿真 的 复杂 性 ,计算 机 的 性 能 和 仿真 设置 ,实时 仿真 可 能 不 总 是 可 行 的 。 
在 非 实时 仿真 中 ,仿真 速度 ( 即 感知 速度 ) 主要 取决 于 两 个 因素 : 浑 染 通道 的 仿真 时 间 
步 长 和 仿真 通道 数 (更 多 细节 参见 仿真 对 话 框 )。 在 实时 仿真 的 情况 下 .仿真 速度 主要 取决 
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于 实时 倍增 系数 ,而 且 还 一 定 程度 地 取决 于 仿真 时 间 步 长 (因为 计算 机 的 计算 能 力 有 限 , 太 
小 的 仿真 时 间 步 长 可 能 与 实时 仿真 不 兼容 )。 如 图 3-94 所 示 ,在 仿真 期 间 , 可 以 使 用 以 下 工 
具 栏 按钮 调整 仿真 速度 。 

以 这 样 的 方式 调整 仿真 速度 ,可 使 得 初始 仿真 时 间 步 长 从 不 增加 (因为 这 可 能 例如 导致 
机 构 的 断 开 ) 。 

默认 情况 下 ,每 个 仿真 循环 由 以 下 顺序 操作 组 成 : 

D 执行 主 脚本 ， 

(2) EGRE SC: 

(3) 线程 呈现 。 

泻 染 操作 将 总 是 增加 仿真 循环 持续 时 间 ,因此 也 减 慢 了 仿真 速度 。 每 个 场景 泻 染 的 主 
脚本 执行 次 数 可 以 定义 (参见 后 面 的 内 容 ), 但 在 某 些 情况 下 这 是 不 够 的 ,因为 浑 染 仍然 会 减 
慢 每 个 仿真 周期 (这 可 能 会 对 实时 要 求 造成 妨碍 )。 对 于 这 些 情况 ,如 图 3-95 所 示 , 可 以 通 
过 用 户 设置 或 通过 以 下 工具 栏 按钮 激活 线程 呈现 模式 。 


ee RU 


图 3-94 仿真 速度 调整 工具 栏 按钮 图 3-95 ”线程 泻 染 工具 栏 按钮 


当 激 活 线程 泻 染 模式 时 ,仿真 循环 将 仅 在 于 执行 主 脚 本 ,因此 仿真 将 以 最 大 速度 运行 。 
泻 染 将 通过 不 同 的 线程 发 生 ,并 且 不 会 减 慢 仿真 任务 。 然 而 当 线 程 呈现 被 激活 时 ,必须 考虑 
以 下 缺点 。 

CD 泻 染 将 异步 发 生 到 仿真 循环 ,并 且 可 能 会 出 现 视觉 上 的 毛刺 。 

(2) 录像 机 不 会 以 恒定 速度 操作 ( 某 些 帧 可 能 会 跳 过 ) 。 

(3) 应 用 程序 的 稳定 性 可 能 会 降低 。 

(4) 一 些 操作 (例如 控 除 对 象 等 ) 需 要 在 能 够 执行 之 前 等 待 呈现 线程 完成 工作 ,反之 亦 
然 。 在 这 些 情况 下 ,循环 可 能 比 在 顺序 泻 染 模式 中 花费 更 多 的 时 间 。 


3.9.1 物理 引擎 的 选择 


V-REP 提供 了 4 类 物理 引擎 ,使 用 户 在 使 用 仿真 功能 时 ,可 以 根据 自身 需求 ,自由 地 从 
一 个 物理 引擎 切换 到 另外 一 个 物理 引擎 。 众 多 的 物理 引擎 存在 的 原因 是 : 物理 仿真 本 身 具 
有 一 定 的 复杂 性 ,选择 不 同 的 物理 引擎 可 以 达到 不 同 的 精度 和 速度 。 

此 外 ,还 可 以 设置 与 形状 相关 的 动力 学 引擎 属性 对 话 框 对 动力 学 引擎 进行 更 细致 的 选 
择 。 如 图 3-96 所 示 ,对 动力 学 引擎 属性 对 话 框 说 明 如 下 。 

材料 名 称 : 材料 的 名 称 。 只 能 对 用 户 定 义 的 材料 进行 修改 。 

重复 材料 : 允许 复制 给 定 材料 。 默 认 材 料 ( 即 非 用 户 定义 材料 ) 首 先 需要 重复 ,然后 才 
能 编辑 它们 。 

V-REP 中 的 四 种 物理 引擎 与 形状 之 间 的 关系 及 物理 的 性 质 如 下 : 
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Property We 
Material name uer. defacitMaterial 
Duplicate material Duplicate. 
EnuDdeiprperte UV | 
Friction (only Bullet V2.78) 7.1000e-01 
Friction (after Bullet V2.78) 7.1000e-01 
Restitution 0.0000e +00 
Lnear dampng 0.0000e+00 
Angular damping 0.0000e +00 
Sticky contact (only Bulet V2. 78) False 
Auto-shrirk convex mesh Faise 
Custom colision margn False 
Custom colision margin factor 1.0000e 01 
| 00E propertes d 
Fricbon 7.1000e 01 
Maximum contacts 64 
Soft ERP 2.0000e 01 
Sof CFM 0.0000e +00 
Lnear damping 0.0000e+00 
vec ee | 
Resttubon 0.0000e +00 
Restitution threshold 5.0000e 01 
Compliance [s^2kg] 1.0000e-08 
Damong [kg/s] 1.00002 +07 
Adhesive force [kg*m/s^2] 0.0000e +00 
Unesr velocity damping kg/s) 0.0000 -400 
Angular velocity damping [kg*m^2/s] 0.0000e+00 
Auto angular dampna enabled False 
Auto angular damping tenson ratio 1.0000e-02 
Son thdmess [m] 0.0000e+00 
Auto-sip enabled False. 
Fast moving 4 me 
Treat pure shape as VxConvexMesh. False. 
Treat convex shape as VxTrangeMeshB.. | False 


图 3-96 与 形状 相关 的 动力 学 引擎 属性 对 话 框 


1. Bullet 的 性 质 

Bullet 的 性 质 是 与 Bullet 物理 库 相 关 的 属性 。 

摩擦 ( 仅 Bullet V2. 785 : 仅 用 于 Bullet V2. 78 的 摩擦 值 。 两 个 碰撞 对 象 将 具有 值 ] * 
值 2 的 组 合 摩擦 值 。 这 不 符合 实际 的 摩擦 系数 。 

摩擦 (在 Bullet V2. 78 之 后 ): 仅 用 于 V2. 78 之 后 的 Bullet 版 本 的 摩擦 值 。 两 个 磁 撞 
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对 象 将 具有 值 1 * 值 2 的 组 合 摩擦 值 。 这 不 符合 实际 的 摩擦 系数 。 

归还 : 归还 价值 。 较 高 的 值 倾向 于 使 碰撞 看 起 来 有 弹性 。 这 不 对 应 于 实际 的 恢复 
系数 。 

线性 阻尼 : 线性 移动 阻尼 值 ,增加 线性 阻力 ,并 可 以 提高 稳定 性 。 

角 阻 尼 : 角 移动 阻 尼 值 ,增加 角 阻力 ,可 以 增加 稳定 性 。 

忒 性 联系 (只 有 Bullet V2. 78): 当选 中 此 项 时 ,接触 点 将 非常 强 , 但 可 能 导致 不 稳定 
性 。 建 议 保持 此 禁用 。Bullet 版 本 在 V2. 78 之 后 不 需要 此 功能 。 

自动 收缩 凸 网 格 : 当 该 项 被 选中 时 , 凸 网 格 将 在 内 部 收缩 ,以便 补偿 碰撞 边际 因子 。 建 
议 保持 此 禁用 。 

自 定义 碰撞 边缘 系数 : 当 自 定义 碰撞 边 距 设置 为 true 时 ,此 因子 将 覆盖 默认 碰撞 边缘 
系数 。 碰 撞 余 量 有 助 于 提高 稳定 性 。 建 议 保持 默认 的 冲突 边际 因子 。 

2. ODE 属性 

ODE 属性 是 与 Open Dynamics Engine 相关 的 属性 。 

ER: 摩擦 值 。 两 个 碰撞 对 象 将 具有 值 1 (82 的 组 合 摩擦 值 。 这 不 符合 实际 的 摩擦 
系数 。 

最 大 联系 人 数 : 要 生成 的 最 大 联系 人 数 。 两 个 碰撞 对 象 将 具有 (valuel 十 value2)/ 2 
的 组 合 最 大 接触 值 。 

软 ERP: 接触 法 的 误差 减 小 参数 ,这 对 于 使 表面 柔软 是 有 用 的 。 两 个 碰撞 对 象 将 具有 
(valuel 十 value2)/2 的 组 合 软 ERP fi. 

软 CFM: 约束 力 混合 参数 的 接触 正常 ,这 对 于 使 表面 柔软 有 用 。 两 个 碰撞 对 象 将 具有 
(valuel 十 value2)/2 的 组 合 软 CFM 值 。 

线性 阻尼 : 线性 移动 阻尼 值 ,增加 线性 阻力 .并 可 以 提高 稳定 性 。 

角 阻尼 : 角 移 动 阻尼 值 ,增加 角 阻 力 , 可 以 增加 稳定 性 。 

3. Vortex 性 质 

Vortex 性 质 是 与 Vortex Dynamics 引擎 相关 的 属性 。 

恢复 : 碰撞 正常 反应 弹性 从 0( 非 弹性 ) 到 1 为 纯 弹 性 。 

RERE: 低 于 该 阀 值 时 ,忽略 恢复 的 速度 阔 值 。 

符合 性 : 材料 柔软 度 (1/ 刚 度 ) 。 

阻尼 : 正常 响应 阻尼 ,用 于 软 接触 ( 低 刚度 )。 

粘 合 力 : 在 接触 处 产生 胶水 。 

线 速度 阻尼 : 人 工 线 速度 阻尼 。 

角速度 阻尼 : 人 工 角速度 阻尼 。 

自动 角度 阻尼 启用 : 在 大 张力 下 刚性 体 的 自我 管理 角 阻 尼 。 不 推荐 用 于 车 轮 或 滚动 
物体 。 

自动 角 阻尼 张力 比 : 自我 管理 角 阻 尼 算 法 的 比例 因子 。 

皮肤 厚度 : 使 皮肤 厚度 更 柔软 (使 抓 握 更 稳定 )。 
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自动 滑 移 启 用 : 自我 管理 接触 黏度 。 

快速 移动 : 在 碰撞 检测 期 间 启 用 附加 检查 ,以 防止 深度 穿 透 或 隧道 效应 。 

把 纯粹 的 形状 当 作 VxConvexMesh: 把 一 个 纯粹 的 形状 作为 一 个 凸 形 。 

将 凸 形 形 状 视 为 VxTriangleMeshBVTree: 将 凸 形 形状 作为 OBB 树 数据 库 处 理 ( 通 常 比 
凸 形 形 状 更 准确 和 稳定 ) 。 

将 随机 形状 视 为 VxTriangleMeshUVGrid: 将 随机 形状 处 理 为 2D 网 格 数据 库 ( 对 于 大 
地 形 数据 库 更 有 效 )。VxTriangleMeshUVGrids 永远 不 会 彼此 冲突 。 

自动 睡眠 : 允许 禁用 非 移动 对 象 以 节省 仿真 时 间 的 功能 。 当 所 有 速度 、 加 速度 都 在 相 
应 的 国 值 内 时 ,部 件 被 认为 是 休眠 。 

阅 值 线 速度 : REE BE. 

阅 值 线性 加 速度 : 线性 加 速度 阔 值 

阅 值 角速度 : 角速度 阔 值 。 

阅 值 角 加 速度 : 角 加 速度 阔 值 。 

阅 值 步 又: 在 再 次 进入 睡眠 之 前 ,部件 必 须 醒 来 的 最 小 步骤 数 。 

线性 主轴 : 摩擦 平面 中 的 轴 。 

轴 方 向 : 指示 主轴 方向 的 矢量 (矢量 将 投影 到 摩擦 平面 中 )。 矢 量 相 对 于 形状 参考 系 。 

摩擦 模型 : 沿 此 轴 的 摩擦 模型 。 

摩擦 系数 : 摩擦 系数 (用 于 缩放 框 和 缩放 框 快速 模型 )。 

静摩擦 标尺 : 静摩擦 / 动 摩擦 比 。 

滑动 : 摩擦 黏度 。 

滑动 : 沿 此 轴 的 摩擦 所 需 的 相对 速度 。 

直线 第 二 轴 : 定义 摩擦 平面 的 第 二 轴 。 

角 主 轴 : 允许 围绕 主轴 (滚动 阻力 ) 添 加 角 摩 擦 。 

角度 副 轴 : 允许 围绕 副 轴 添 加 角 摩 擦 (滚动 阻力 ) 。 

角 法 向 轴 : 允许 围绕 法 向 轴 添 加 角 摩擦 (旋转 阻力 )。 

4. Newton 引擎 的 属性 

静摩擦 : 静摩擦 系数 。 两 个 碰撞 对 象 将 具有 值 1 * 值 2 的 组 合 摩擦 值 。 

动 摩擦 : 动 摩擦 系数 。 两 个 碰撞 对 象 将 具有 值 1 * 值 2 的 组 合 摩擦 值 。 

归还 : 归还 系数 。 较 高 的 值 倾向 于 使 碰撞 看 起 来 有 弹性 。 两 个 碰撞 对 象 将 具有 valuel 十 
value2 的 组 合 恢复 值 。 

线性 拖 动 : 线性 拖 动 值 ,这 可 以 提高 稳定 性 。 

角度 拖 动 : 角度 拖 动 值 ,这 可 以 提高 稳定 性 。 

快速 移动 : 在 碰撞 检测 期 间 启 用 附加 检查 ,以 防止 深度 穿 透 或 隧道 效应 。 

5. Vortex 的 附加 信息 

材质 属性 定义 单个 形状 在 与 另 一 个 形状 碰撞 时 的 行为 。 碰 撞 涉 及 两 种 形状 及 其 相应 的 
材料 性 质 。 接 触 材 料 将 这 两 种 材料 性 质 组 合成 用 于 在 执行 仿真 期 间 产 生 两 个 碰撞 形状 之 间 
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的 接触 力 的 合并 的 一 组 性 质 。 合 并 两 种 联系 材料 的 规则 如 下 : 

COD 符合 性 : 采取 最 兼容 和 相关 的 阻尼 和 恢复 。 

D 粘 合力 : 使 用 较 大 的 值 。 

(3) 皮肤 厚度 : 使 用 较 大 的 值 。 

(4) 摩擦 模型 如 果 两 个 模型 不 同 , 优 先 级 如 下 : none. boxProportional. scaledBox， 
scaledBoxFast,box,neutral。 摩 擦 模型 定义 如 下 : 

。 无 : 无 摩擦 。 

* Box 比例 : n/a. 

e ScaledBox; 摩擦 边界 三 法 向 力 * 摩擦 系数 。 

。 盒 : n/a, 

。 中 性 : 最 低 优先 级 ,因此 使 用 其 他 模型 。 如 果 两 者 都 是 中 性 的 , 它 是 无 摩擦 的 。 

如 果 两 个 模型 相同 , 则 使 用 最 低 的 摩擦 系数 。 如 果 摩 擦 不 是 各 向 同性 的 , 则 可 以 在 限定 
摩擦 平面 (线性 主轴 和 副 轴 ) 的 2 个 方向 上 提供 不 同 的 摩擦 特性 。 轴 方向 是 对 象 的 局 部 框架 
中 的 向 量 。 它 在 摩擦 平面 中 的 投影 定义 了 线性 主 方向 。 轴 方向 仅 用 于 各 向 异性 材料 。 如 果 
两 种 各 向 异性 材料 相互 作用 , 则 将 使 用 它们 中 的 一 种 ,用 户 当 前 不 能 优先 考虑 哪 一 种 。 


3.9.2 仿真 参数 的 设置 


仿真 参数 设置 的 对 话 框 可 以 通过 依次 单 击 Menu bar GE C F5), Simulation (仿真)、 
Simulation settings( 仿 真 设置 ) 或 者 单 击 工具 栏 2 中 仿真 设置 打开 。 如 图 3-97 所 示 , 它 可 以 
设置 仿真 的 时 间 步 长 .每 帧 泻 染 的 仿真 ,仿真 时 间 等 。 


Main settings 


dt-50 ms (default) | Time step [40.0500 — 


Simulation passes per frame (pof) 1 
Pause when simulation time higher than 10. 00 
Pause on script error 
Full sereen at simulation 


Real-time simulation 
Enabled Multiplication faef1 00 — 
Try catching up when behind 


At simulation end 


Vi Reset scene to initial state 
V Remove new objects 


图 3-97 仿真 设置 对 话 框 
如 图 3-97 所 示 ,对 仿真 设置 对 话 框 的 说 明 如 下 。 
时 间 步 长 : 仿真 时 间 步 长 。 每 次 执行 主 脚 本 时 .仿真 时 间 都 会 按照 仿真 时 间 步 长 递增 。 
使 用 大 的 时 间 步 长 可 实现 快速 但 不 准确 /不 稳定 的 仿真 。 另 一 方面 ,小 时 间 步 长 (通常 ) 可 实 
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现 更 精确 的 仿真 ,但 将 需要 更 多 的 时 间 。 强 烈 建议 保留 默认 时 间 步 长 。 

每 帧 的 仿真 传递 (ppf): 一 个 泻 染 传递 的 仿真 传递 数 。 值 10 表示 在 屏幕 刷新 之 前 , 主 
脚本 执行 10 次 (10 个 仿真 步骤 ) 。 

当 仿 真 时 间 高 于 XX 时 ,暂停 : 允许 指定 仿真 暂停 的 仿真 时 间 ( 例 如 ,能 够 在 特定 仿真 
时 间 分 析 一 些 结果 ) 。 

暂停 脚本 错误 : 如 果 启 用 , 则 当 脚 本 错误 发 生 时 ,仿真 将 暂停 。 

仿真 启动 时 的 全 屏 : 如 果 启 用 , 则 仿真 在 全 屏 模式 下 启动 。 请 注意 ,在 全 屏 模 式 下 ,对 
话 框 和 消息 不 会 出 现 或 将 不 可 见 ,只 有 鼠标 左 键 将 处 于 活动 状态 。 因 为 这 个 原因 ,只 有 在 场 
景 被 正确 配置 和 最 终 时 才 推 荐 该 模式 。 全 屏 模 式 可 以 使 用 Esc 键 ,并 在 仿真 期 间 通过 布尔 
参数 sim_booparam_fullscreen 切换 。Unler Linux 和 MacOS 全 屏 模 式 可 能 仅 部 分 支持 ,并 
且 切 换 回 正 常 模式 可 能 会 在 某 些 系统 上 失败 。 

实时 仿真 ,乘法 因子 : 如 果 选 择 , 则 仿真 时 间 将 尽量 跟随 实时 。 乘 法 因子 X 将 试图 运 
行 仿真 比 实时 快 X 倍 。 

尝试 在 后 面 赶 上 : 在 实时 仿真 期 间 , 可 能 发 生 仿真 时 间 不 能 跟随 实时 (例如 ,由 于 一 些 
暂时 的 计算 )。 在 这 种 情况 下 ,如 果 选 中 此 复 选 框 , 则 仿真 时 间 将 尝试 赶 上 损失 的 时 间 ( 例 
如 , 当 计 算 负载 再 次 减 小 时 ) ,这 导致 明显 的 加 速 。 

将 场景 重 置 为 初始 状态 : 当选 择 时 ,所 有 对 象 将 被 重 置 为 其 初始 状态 ,包括 对 象 局 部 位 
置 .局 部 方向 及 其 父 对 象 ( 只 要 对 象 未 被 修改 (否则 为 缩放 )) .关节 和 路 径 固 有 位 置 .浮动 视 
图 位 置 和 大 小 等 。 这 意味 着 下 一 个 仿真 运行 将 以 与 前 一 个 相同 的 方式 执行 ,除非 进行 了 大 
的 改变 (如 形状 缩放 、 对 象 移 除 等 )。 此 项 目 忽略 一 些 次 要 设置 。 

删除 新 对 象 : 选择 后 ,在 仿真 运行 期 间 添 加 的 场景 对 象 将 在 仿真 结束 时 删除 。 


3.9.3 仿真 的 控制 


仿真 的 控制 主要 跟 工 具 栏 中 的 5 个 按钮 有 关 。 选 择 Start/resume simulation 可 对 已 经 
构建 好 的 机 器 人 模型 及 其 工作 环境 进行 仿真 。 在 对 默认 的 模型 未 做 出 其 他 修改 时 ,机 器 人 
将 使 用 V-REP 内 置 的 运动 规划 和 递 运动 学 的 算法 进行 运动 。 在 机 器 人 运动 的 过 程 中 ,在 
场景 的 上 方 还 将 实时 地 呈现 机 器 人 运动 的 信息 文本 。 选 择 Pause simulation 可 暂停 仿真 过 
程 ,选择 Stop simulation 可 结束 仿真 过 程 ,机 
器 人 模型 及 其 他 场景 将 恢复 到 起 始 的 状态 。 在 一 
V-REP 仿真 过 程 中 , 单 击 speed up simulation. M E C] Shov button states — 
可 以 对 仿真 过 程 进行 加 速 , 单 击 slow down G dae een 


Location — BE) TaEHAT-EEP Fan SDU/recordins| | Select 


simulation, ,可 以 对 仿真 过 程 进行 减速 。 1 — | displayed frames result in | recorded frase 


Output trpe AVI/M 263+ / H 263-1998 / H 263 


此 外 ,V-REP 还 提供 了 录像 功能 ,通过 依 V Automatic frame rat Prase rate [f/s) [20 
次 单 击 Menu bar GE 3& £2, Tools (工具 )、 Video resolution 13221606 


Video recorder( 录 像 机) 打开 录像 机 对 话 框 ， 


图 3-98 ”录像 机 设置 对 话 框 
如 图 3-98 所 示 。 但 选择 开始 记录 或 仿真 开始 
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时 , 它 将 记录 机 器 人 仿真 的 动画 ; 当 仿 真 结束 时 , 它 将 自动 地 把 动画 保存 为 avi 的 视频 格式 
至 路 径 V-REP3\V-REP_PRO_EDU\recording. avi 中 。 


3.10 V-REP 的 具体 例子 


3.10.1 机 械 臂 模型 的 构建 


1. 构建 可 视 形 状 

当 构 建 一 个 新 模型 时 ,首先 ,我 们 只 处 理 它 的 视觉 方面 ,动态 方面 (其 潜在 的 更 简化 / 优 
化 模型 )、 关 节 ,传感器 等 将 在 稍 后 阶段 处 理 。 

现在 可 以 在 V-REP 中 通过 依次 单 击 菜单 栏 , 添 加、 原始 形状 、 直 接 创 建 原始 形状 。 执 
行 此 操作 时 ,我 们 可 以 选择 创建 纯 形 状 或 常规 形状 。 纯 形状 将 被 优化 用 于 动态 交互 ,并 且 还 
会 被 直接 动态 地 启用 ( 即 下 降 、 碰 撞 , 但 是 这 可 以 在 稍 后 阶段 被 禁用 )。 原 始 形状 将 是 简单 的 
网 格 ,可 能 不 包含 足够 的 细节 或 几何 精度 。 在 这 种 情况 下 ,可 以 选择 其 他 方法 : 从 外 部 应 用 
程序 导入 网 格 。 

当 从 外 部 应 用 程序 导入 CAD 数据 时 ,最 重要 的 是 要 确保 CAD. 模型 不 会 太 繁 重 , 即 不 
包含 太 多 的 三 角形 。 这 个 要 求 很 重要 ,因为 重型 模型 显示 缓慢 ,并 且 也 可 能 减 慢 在 稍 后 阶段 
使 用 的 各 种 计算 模块 (例如 最 小 距离 计算 或 动态 )。 如 图 3-99 所 示 , 下 面 的 例子 通常 是 难以 
实现 的 。 


图 3-99 复杂 CAD 数 据 ( 实 体 和 线 框 ) 


上 例 中 CAD 模型 的 数据 量 非 常 大 : 它 包 含 许多 三 角形 (超过 47000) ,如果 只 在 空 场景 
中 使 用 它 的 单个 实例 ,这 是 可 以 做 到 的 。 但 大 多 数 时 候 , 你 可 能 想 要 仿真 同一 个 机 器 人 的 几 
个 实例 ,连接 各 种 类 型 的 组 件 ,也 许 这 些 机 器 人 还 会 与 其 他 机 器 人 、 设 备 或 环境 交互 。 在 这 
种 情况 下 ,仿真 场景 可 能 变 得 很 慢 。 一 般 来 说 ,建议 对 一 个 由 不 超过 20000 个 三 角形 组 成 的 
机 器 人 进行 建 模 ,但 大 多 数 时候 5000 一 10000 三 角形 就 可 以 很 好 地 满足 各 种 需求 。 记 住 : 
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不 管 在 什么 情况 下 ,三 角形 数量 越 少 越 好 。 

为 何 上 面 的 模型 会 如 此 繁重 ? 首先 ,包含 孔 和 小 细节 的 模型 将 需要 更 多 的 三 角形 面 以 
获得 正确 的 表示 。 因 此 ,如 果 可 能 ,请 尝试 从 原始 模型 数据 中 删除 所 有 的 孔 、 螺 丝 、 对 象 的 内 
部 结构 等 。 如 果 将 原始 模型 数据 表示 为 参数 曲面 /对 象 , 则 大 多 数 时 间 会 花 在 选择 项 目 和 删 
除 项 目 ( 例 如 在 Solidworks 中 ) 的 简单 问题 上 。 第 二 个 重要 步骤 是 以 有 限 的 精度 导出 原始 
数据 : 大 多 数 CAD 应 用 程序 允许 你 指定 导出 网 格 的 精确 程度 。 当 绘图 由 大 小 对 象 共同 组 
成 时 ,有 必要 用 多 个 步骤 导出 对 象 ,这 是 为 了 避免 大 对 象 具 有 过 于 精确 定义 ( 太 多 的 三 角形 ) 
和 小 对 象 太 粗略 定义 ( 太 少 的 三 角形 ) ,应 当 : 首先 导出 大 对 象 (通过 调整 所 需 的 精度 设置 )， 
然后 再 导出 小 对 象 (通过 调整 精度 设置 ) 。 

V-REP 支持 当前 以 下 CAD 数据 格式 ，OBJ, STL, DXF, 3DS( 仅 限 Windows) 和 
Collada, URDF 也 受 支 持 , 但 在 这 里 不 涉及 ,因为 它 不 是 一 个 纯粹 的 基于 网 格 的 文件 格式 。 

现在 假设 我 们 已 经 应 用 了 所 有 可 能 的 简化 。 如 图 3-100 所 示 ,在 导入 后 ,我 们 可 能 仍然 
会 遇 到 太 繁 重 的 网 格 。 


图 3-100 导入 的 CAD 数据 


可 以 注意 到 : 整个 机 器 人 是 作为 单个 网 格 导入 的 。 我 们 稍 后 将 看 到 如 何 适当 划分 它 的 
方法 。 注 意 导 和 人 网 格 的 方向 : 最 好 是 保持 方向 ,直到 整个 模型 被 建立 ,因为 ,如 果 在 以 后 的 
阶段 中 我 们 要 导入 与 同一 机 器 人 相关 的 其 他 项 目 , 它 们 将 自动 具有 相对 于 原始 网 格 的 正确 
位 置 /取向 。 

在 这 个 阶段 ,有 几 个 功能 可 供 我 们 使 用 ,以 简化 网 格 : 

(1) 自动 网 格 划分 : 允许 为 未 通过 公共 边缘 连 杆 在 一 起 的 所 有 元 素 生成 新 形状 。 这 并 
不 总 是 适用 于 选 定 的 网 格 ,但 是 值得 一 试 , 因 为 在 网 格 元 素 上 的 工作 会 给 我 们 带 来 更 多 的 麻 
烦 , 如 果 我 们 不 得 不 同时 处 理 所 有 的 元 素 。 可 以 通过 依次 单 击 菜单 栏 编 辑 . 分 组 /合并 、 分 
割 所 选 形状 以 访问 该 功能 。 有 时 ,网 格 将 被 划分 得 超过 预期 。 在 这 种 情况 下 ,简单 地 合并 罗 
辑 上 属于 一 体 的 元 素 ( 即 具有 相同 的 视觉 属性 并 且 是 同一 连 杆 的 一 部 分 ) 使 其 成 为 一 个 单一 
的 形状 (通过 依次 单 击 菜单 栏 编辑 、 分 组 /合并 、 合 并 选 定 的 形状 )。 

(2) 提取 凸 包 : 允许 通过 将 网 格 转换 为 凸 包 来 简化 网 格 。 该 功能 可 以 通过 依次 单 击 菜 
单 栏 ` 编 辑 ` 变 形 选择 成 凸 形 来 访问 。 
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(3) 抽取 网 格 : 允许 减少 网 格 中 包含 的 三 角形 的 数量 。 可 以 通过 依次 单 击 菜单 栏 、 编 
辑 、 抽 取 所 选 形 状 以 访问 该 功能 。 

CO 删除 网 格 内 部 : 允许 通过 删除 网 格 简化 网 格 。 此 功能 基于 视觉 传感器 ,并 且 可 能 
根据 所 选 设置 提供 不 同 结果 。 可 以 通过 依次 单 击 菜单 栏 编辑 ,在 选 定形 状 内 提取 以 访问 该 
功能 。 

上 面 的 函数 的 应 用 没有 预定 义 的 顺序 (除了 列表 中 的 第 一 个 项 目 总 是 应 该 先 被 尝试 )， 
它 在 很 大 程度 上 取决 于 我 们 试图 简化 的 网 格 几何 。 如 图 3-101 所 示 , 它 演示 了 应 用 于 导入 
网 格 的 上 述 功能 (假设 列表 中 的 第 一 个 项 目 并 不 适用 )。 


图 3-101 囊 体 ,抽取 网 格 ,并 在 里 面 提取 


注意 : 在 这 个 阶段 , 凸 包 不 能 帮助 我 们 。 我 们 决定 先 使 用 网 格 抽取 功能 ,并 运行 函数 两 
次 ,以 便 将 三 角形 的 数量 除 以 总 数 50。 一 旦 完成 这 一 步 ,我 们 将 提取 简化 形状 的 内 部 并 丢 
弃 它 。 我 们 最 终 会 得 到 一 个 总 共 包 含 2660 个 三 角形 的 网 格 (原始 导入 的 网 格 包含 超过 
136 000 个 三 角形 )。 在 形状 几何 对 话 框 中 可 以 看 到 形状 所 包含 的 三 角形 /顶点 的 数量 。 
2660 是 构建 一 个 完整 的 机 器 人 模型 需要 的 最 少 的 三 角形 数量 ,但 是 视觉 外 观 可 能 受到 一 点 
点 影响 。 

在 这 个 阶段 ,可 以 开始 将 机 器 人 分 成 单独 的 连 杆 ( 记 住 ,目前 整个 机 器 人 只 有 一 个 形 
状 )。 可 以 通过 两 种 不 同 的 方式 做 到 这 一 点 : 

(1) 自动 网 格 划分 : 如 上 所 述 ,这 个 功能 将 检查 形状 ,并 生成 一 个 新 形状 的 所 有 元 素 ， 
并 且 没 有 通过 共同 的 边缘 连 杆 在 一 起 。 这 个 方法 并 不 总 是 有 效 , 但 值得 一 试 。 可 以 通过 依 
次 单 击 菜单 栏 编辑、 分 组 /合并 ,分 割 所 选 形 状 以 访问 该 功能 。 
(2) 手动 网 格 划分 : 通过 三 角 编 辑 模式 ,可 以 手动 选择 在 逻辑 上 属于 一 体 的 三 角形 , 然 
后 单 击 提取 形状 。 这 将 在 场景 中 生成 新 的 形状 。 在 该 操作 后 将 删除 选 定 的 三 角形 。 

如 图 3-102 所 示 为 分 隔 的 网 格 。 

现在 ,可 以 进一步 细 化 /简化 个 别 形状 。 有 时 ,如 果 使 用 凸 包 , 形 状 可 能 看 起 来 更 好 。 其 
他 时 候 ,你 将 不 得 不 迭代 使 用 上 面 描述 的 几 种 技术 ,以 获得 所 需 的 结果 。 以 如 图 3-103 所 示 
的 网 格 为 例 。 

上 面 的 形状 的 问题 在 于 ,不 能 很 好 地 简化 ,因为 它 包 含 了 洞 。 所 以 必须 通过 形状 编辑 模 
式 这 种 更 复杂 的 方式 ,其 中 可 以 提取 逻辑 上 属于 同一 个 凸 子 实体 的 各 个 元 素 。 这 个 过 程 可 
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图 3-102 分 隔 的 网 格 


以 进行 几 次 迭代 ,首先 提取 3 个 近似 凸 元 素 。 现 在 ,忽略 作为 两 个 孔 的 一 部 分 的 三 角形 。 在 
形状 编辑 模式 下 编辑 形状 时 ,可 以 方便 地 切换 可 视图 层 , 以 查看 其 他 场景 项 目 覆 盖 的 内 容 ， 
如 图 3-104 所 示 。 


图 3-103 导入 的 网 格 图 3-104 得 到 的 图 形 


最 终 形成 了 三 种 形状 ,但 其 中 两 种 需要 进一步 改进 。 现 在 可 以 删除 作为 孔 的 一 部 分 的 
三 角形 。 最 后 ,为 3 个 形状 单独 提取 凸 包 ,然后 通过 依次 单 击 菜单 栏 编辑 ,分 组 /合并 、 合 并 
所 选 形状 以 将 它们 合并 回来 ,得 到 的 模型 如 图 3-105 所 示 。 

在 V-REP 中 ,可 以 启用 /禁用 每 个 形状 的 边缘 显示 。 还 可 以 指定 边缘 显示 的 角度 。 类 
似 的 参数 是 阴影 角度 ,其 指示 形状 的 显示 方式 。 可 以 在 形状 属性 中 调整 这 些 参数 以 及 诸如 
形状 颜色 的 其 他 几 个 参数 。 形 状 有 各 种 各 样 的 形式 ,在 本 部 分 内 容 中 ,我 们 只 处 理 到 现在 的 
简单 形状 : 一 个 简单 的 形状 有 一 组 单一 的 视觉 属性 ( 即 一 种 颜色 ,一 个 阴影 角度 等 )。 如 果 
合并 两 个 形状 ,那么 结果 将 是 一 个 简单 的 形状 。 还 可 以 对 形状 进行 分 组 ,在 这 种 情况 下 ,每 
个 形状 将 保留 其 视觉 属性 。 

下 一 步 可 以 合并 逻辑 上 属于 一 体 的 元 素 (如 果 它 们 是 同一 刚性 元 素 的 一 部 分 ,并 且 它 们 
具有 相同 的 视觉 属性 ) ,然后 改变 各 种 元 素 的 视觉 属性 。 最 简单 的 方法 是 调整 具有 不 同 颜 色 
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mv 


图 3-105 得 到 的 模型 


和 视觉 属性 的 几 个 形状 ,如 果 使 用 特定 字符 串 
命名 颜色 ,可 以 在 随后 的 工作 中 轻松 地 以 编程 
的 方式 更 改 该 颜色 (即使 形状 是 复合 形状 的 一 
部 分 )。 然 后 ,选择 具有 相同 视觉 属性 的 所 有 
形状 ,控制 选择 已 经 调整 的 形状 , 单 击 形状 属 
性 中 的 “应 用 于 选择 ”, 一 次 用 于 “颜色 ”属性 ， 
一 次 用 于 “其 他 属性 ”, 这 将 转化 所 有 视觉 属性 
到 选 定 的 形状 (包括 你 提供 的 颜色 名 称 )。 最 
终 得 到 17 个 独特 的 形状 ,如 图 3-106 所 示 。 

现在 我 们 可 以 通过 依次 单 击 菜单 栏 ,编辑 分组/ 合并、 分 组 所 选 形状 以 将 属于 同一 连 杆 
一 部 分 的 形状 分 组 。 最 终 得 到 7 种 形状 : 即 机 器 人 的 基 座 (或 机 器 人 的 层次 树 的 基 座 ) 和 6 
个 移动 连 杆 。 正 确 命名 对 象 也 很 重要 : 你 可 以 双击 场景 层次 结构 中 的 对 象 名 称 进行 修改 。 
基 座 应 始终 是 机 器 人 或 模型 名 称 ,其 他 对 象 应 始终 包含 基 座 对 象 名 称 , 如 : robot (base), 
robot linkl,robot proximitySensor 等 。 通 过 默认 设置 ,形状 将 分 配给 可 视 层 1, 但 可 以 在 
对 象 公共 属性 中 更 改 。 默 认 情 况 下 , 仅 激活 场景 的 可 见 性 图 层 1 一 8。 我 们 现在 有 以 下 模型 
(模型 ResizableFloor_5_25 在 模型 属性 对 话 框 中 暂时 使 不 可 见 ) ,如 图 3-107 所 示 o 

创建 或 修改 形状 时 ,V-REP 将 自动 设置 其 参考 框架 位 置 和 方向 。 形 状 的 参考 框架 将 始 
终 位 于 形状 的 几何 中 心 。 默 认 选 择 的 框架 方向 会 使 形状 的 边界 框 保持 尽 可 能 小 。 但 显示 效 
果 并 不 总 是 令 人 满意 ,不 过 可 以 随时 重新 定向 形状 的 参考 系 。 通 过 依次 单 击 菜单 栏 编辑 、 
重 定向 边界 框 .与 世界 的 参考 框架 ,可 以 重新 定向 我 们 创建 的 所 有 形状 的 参考 框架 。 形 状 几 
何 对 话 框 中 有 更 多 选项 可 以 用 来 重新 定向 参考 系 。 

2. 建立 关节 

现在 关注 关节 /电机 。 大 多 数 时 候 ,我 们 知道 每 个 关节 的 确切 位 置 和 方向 。 在 这 种 情况 
下 ,我们 只 需 通 过 依次 单 击 菜单 栏 添 加 、 关 节 , 然 后 使 用 坐标 和 变换 对 话 框 更 改 它们 的 位 置 
和 方向 。 有 时 候 我 们 只 有 Denavit-Hartenberg CH] DH) 参 数 。 在 这 种 情况 下 ,可 以 通过 


图 3-106 调整 后 的 视觉 属性 
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网 格 。 大 多 数 时 候 , 我 
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图 3-107 单个 元 素 组 合成 的 机 器 人 


-Hartenberg joint creator. ttm 中 的 工具 模型 在 模型 浏览 器 中 建立 我 
们 的 关节 。 没 有 关于 关节 位 置 和 方向 的 信息 时 ,我 们 需要 从 导入 的 网 格 中 提取 它们 。 假 设 
我 们 打开 一 个 新 的 场景 ,并 再 次 导入 原始 的 CAD 数据 ,而 不 是 处 理 经 过 修改 的 、 更 近似 的 
们 可 以 从 原始 网 格 中 提取 网 格 或 原始 形状 。 第 一 步 是 细 分 原始 网 格 。 
使 用 三 角形 编辑 模式 。 假 设 我 们 可 以 分 割 原始 网 格 ,现在 有 可 以 看 到 
的 更 小 的 对 象 。 我 们 寻找 可 以 用 作 参 考 的 旋转 形状 ,以 便 在 其 位 置 创建 关节 ,同时 具有 相同 
有 不 需要 的 对 象 。 有 时 需要 跨越 几 个 打开 的 场景 ,以 便 获得 更 容易 的 
中 ,我 们 首先 关注 机 器 人 的 基 座 : 它 包 含 一 个 具有 第 一 关节 正确 位 置 
的 圆柱 体 。 如 图 3-108 所 示 ,在 三 角形 编辑 模式 得 到 的 机 器 人 基 座 模型 。 


图 3-108 机 器 人 基 座 : 正常 和 三 角形 编辑 模式 可 视 化 
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通过 页 面 选择 器 工具 栏 按钮 更 改 摄像 机 视图 ,以 便 从 侧面 查看 对 象 。 适 合 视图 的 工具 
栏 按钮 可 以 派 上 用 场 ,以 便 在 编辑 中 正确 地 构建 对 象 。 然 后 我 们 切换 到 顶点 编辑 模式 ,并 选 
择 属 于 项 盘 的 所 有 顶点。 记 住 ,通过 切换 一 些 图 层 的 开 / 关 ,可 以 隐藏 场景 中 的 其 他 对 象 。 
然后 我 们 切换 回 三 角形 编辑 模式 ,得 到 如 图 3-109 所 示 的 模型 。 


图 3-109 ”所 选项 盘 、 顶 点 编辑 模式 和 三 角 编辑 模式 


现在 选择 Extract cylinder( Extract shape) ,Extract shape 也 可 以 在 这 种 情况 下 工作 ， 
这 命令 只 是 根据 所 选 的 三 角形 在 场景 中 创建 了 一 个 柱 形 。 我 们 离开 编辑 模式 并 放弃 更 改 。 
现在 我 们 通过 依次 单 击 菜单 栏 添 加 、 关 节 、 旋 转 添加 一 个 旋转 关节 保持 选择 ,然后 控制 选择 
提取 的 圆柱 形状 。 在 坐标 和 变换 对 话 框 中 ,我 们 单 击 位 置 /平移 按钮 ,然后 在 对 象 /项 目 位 置 
部 分 , 单 击 应 用 于 选择 (编辑 框 下 面 的 ): 这 基本 上 将 z/yV/z= 
圆柱 的 位 置 复制 到 关节 中 。 现 在 两 个 位 置 相 同 。 然 后 单 击 方 
向 /旋转 按钮 ,在 对 象 /项 目 定向 部 分 中 , 单 击 应 用 于 选择 : 我 
们 所 选 对 象 的 方向 现在 也 是 相同 的 。 有 时 ,我 们 需要 围绕 其 
自身 的 参考 系 附加 地 旋转 关节 约 907/1807 . LL RE 3K f$ 1E Bf hY 
取向 或 旋转 方向 。 如 果 需 要 ,我们 可 以 在 对 象 /项 目 循环 操作 
部 分 执行 此 操作 (在 这 种 情况 下 ,不 要 忘记 单 击 自 己 的 框架 按 
HD. 。 以 类 似 的 方式 ,我 们 也 可 以 沿 着 它 的 轴 移 动 关节 ,或 者 
做 更 复杂 的 操作 。 得 到 的 模型 如 图 3-110 所 示 。 图 3-110 关节 在 正确 的 位 置 ， 

现在 把 关节 复制 到 原来 的 场景 ,并 保存 它 。 对 机 器 人 中 具有 正确 的 方向 
的 所 有 关节 重复 上 述 过 程 ,然后 重 命名 。 在 关节 属性 中 ,我 们 
还 使 所 有 关节 略 长 ,以 便 能 够 看 到 它们 。 默 认 设置 下 ,关节 将 被 分 配给 可 视 层 2, 不 过 这 可 
以 在 对 象 公共 属性 中 更 改 。 现 在 我 们 将 所 有 关节 分 配给 可 见 层 10, 然 后 暂时 使 能 用 于 场景 
的 可 见 层 10 也 将 这 些 关节 可 视 化 (默认 情况 下 , 仅 为 场景 激活 可 见 层 1 一 8)。 如 图 3-111 所 
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示 为 得 到 的 模型 。(ResizableFloor 5 25 模型 暂时 在 模型 属性 对 话 框 中 不 可 见 ) 。 


“Se be T 
SR Es 


图 3-111 配置 正确 的 关节 


现在 可 以 开始 构建 模型 层次 结构 并 完成 模型 定义 。 但 是 如 果 想 要 动态 启用 opur 机 器 
人 ,那么 还 需要 一 个 额外 的 中 间 步 又 。 

3. 构建 动态 形状 

如 果 我 们 希望 我 们 的 机 器 人 被 动态 启用 , 即 对 碰撞 、 跌 落 等 作出 反应 ,那么 我 们 需要 适 
当地 创建 /配置 形状 。 形 状 可 以 是 : 

CD 动态 或 静态 : 动态 (或 非 静 态 ) 形 状 将 会 下 降 并 受到 外 力 /力矩 的 影响 。 另 一 方面 ， 
静态 (或 非 动态 ) 形 状 将 保持 在 适当 位 置 ,或 者 遵循 其 父 级 在 场景 层次 结构 中 的 移动 。 

(2) 可 应 答 的 或 不 可 应 答 的 : 可 应 答 的 形状 将 引起 与 其 他 可 应 答 的 形状 的 碰撞 反应 。 
它们 是 动态 的 ,它们 和 它们 的 碰撞 物 的 运动 将 会 受到 影响 。 另 一 方面 ,如 果 不 可 响应 形状 与 
其 他 形状 碰撞 , 则 不 能 计算 碰撞 响应 。 

可 响应 的 形状 应 该 尽 可 能 简单 .以 便 允 许 快 速 和 稳定 的 仿真 。 物 理 引 擎 将 能 够 仿真 以 
下 5 种 类 型 的 具有 不 同 程度 的 速度 和 稳定 性 的 形状 : 

CD 纯 形 状 : 纯 形 状 将 是 稳定 的 ,并 由 物理 引擎 处 理 时 非常 有 效 。 但 是 , 纯 形 状 在 几何 
形状 上 受到 限制 ,主要 是 立方 体 、 圆 柱 体 和 球体 。 如 果 可 能 ,可 以 用 于 那些 长 时 间 与 其 他 物 
品 接触 的 物品 (例如 类 人 机 器 人 的 脚 ,连续 机 械 臂 的 基 座 ,夹具 的 手指 等 )。 纯 形状 可 以 通过 
依次 单 击 菜单 栏 ` 添 加、 原始 形状 以 创建 。 

(2) 纯 复 合 形状 : 纯 复合 形状 是 几 种 纯 形 状 的 分 组 。 它 与 纯 形 状 共享 类 似 的 属性 。 可 
以 通过 依次 单 击 菜单 栏 . 编 辑 . 分组/ 合并、 分 组 所 选 形状 以 分 组 几 个 纯 形 状 生成 纯 复 合 

(3) PUER: 凸 形状 会 稍微 不 稳定 ,并 且 当 由 物理 引擎 处 理 时 需要 花费 更 多 的 计算 时 
间 。 它 可 以 有 比 纯 形状 更 一 般 的 几何 特性 (唯一 的 要 求 : 它 需 要 是 凸 的 )。 如 果 可 能 ,对 于 
偶尔 与 其 他 部 件 ( 例 如 机 器 人 的 各 种 连 杆 ) 接 触 的 部 件 , 可 以 使 用 凸 面 形状 。 凸 面 形状 可 以 
通过 依次 单 击 菜单 栏 , 添 加 、 选 择 凸 面 或 通过 依次 单 击 菜单 栏 , 编 辑 、 变 形 选 择 为 凸 形 以 
生成 。 
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(4) 复合 凸 形状 或 凸 分 解 形状 : 凸 分 解 形 状 是 几 种 凸 形 形 状 的 分 组 。 它 与 凸 形 形 状 和 
共享 类 似 的 属性 。 通 过 依次 单 击 菜单 栏 编 辑 ` 分 组 /合并 、 分 组 所 选 形状 以 将 多 个 凸 形 分 
组 ,通过 依次 单 击 菜单 栏 、 添 加、 选择 的 凸 形 分 解 可 以 生成 凸 形 分 解 形状 ,或 者 通过 依次 单 击 
菜单 栏 编辑 、 变 形 选择 到 其 凸 分 解 来 实现 。 

O) 随机 形状 : 随机 形状 既 不 是 凸 形 的 也 不 是 纯 的 形状 。 它 通常 具有 较 差 的 性 能 (如 
计算 速度 和 稳定 性 ) 。 应 该 尽 可 能 避免 使 用 太 多 的 随机 形状 。 

因此 ,优先 顺序 从 高 到 低 依次 是 : 纯 形状 , 纯 复合 形状 , 凸 形 形 状 ,复合 凸 形状 ,以 及 随 
机 形状 。 在 我 们 想 要 构建 的 机 器 人 中 ,将 机 器 人 的 基部 作为 纯 圆 柱 体 ,并 且 其 他 连 杆 作为 凸 
形 或 分 解 形状 。 

我 们 可 以 使 用 动态 启用 的 形状 作为 机 器 人 的 可 见 部 分 ,但 这 可 能 看 起 来 不 够 好 。 所 以 ， 
相反 地 ,将 为 第 一 部 分 创建 的 可 视 形状 构建 一 个 动态 副本 ,并 隐藏 掉 : 隐藏 部 分 将 代表 动态 
模型 ,并 可 以 完全 由 物理 引擎 使 用 ,而 可 见 部 分 将 用 于 可 视 化 ,但 也 用 于 最 小 距离 计算 ,接近 
传感器 检测 等 。 

我 们 选择 机 器 人 对 象 ,将 其 复制 并 粘贴 到 一 个 新 场景 (为 了 保持 原始 模型 不 变 ) ,并 启动 
三 角形 编辑 模式 。 如 果 对 象 是 机 器 人 复合 形状 ,我 们 首先 不 得 不 取消 它 的 组 合 (通过 依次 单 
击 菜单 栏 ` 编 辑 ` 分 组 /合并 .取消 组 合 所 选 形状 ) ,然后 合并 各 自 的 形状 (通过 依次 单 击 菜单 
FE 编辑 ` 分 组 /合并 、 合 并 所 选 形状 ) ,然后 才能 启动 三 角形 编辑 模式 。 现 在 我 们 选择 代表 电 
源 线 的 几 个 三 角形 ,并 删除 它们 。 然 后 选择 该 形状 中 的 所 有 三 角形 ,再 单 击 提取 柱 面 。 我 们 
现在 可 以 离开 编辑 模式 ,如 图 3-112 所 示 ,基础 对 象 已 经 表示 为 纯 圆柱 体 。 


À l L 上 


图 3-112 纯 圆 简 生 成 程序 (在 三 角 编 辑 模式 ) 


我 们 重 命名 新 形状 (双击 它 在 场景 层次 结构 中 的 名 称 ) 为 robot_dyn, 将 其 分 配给 可 见 
性 层 9, 然 后 将 其 复制 到 原始 场景 。 其 余 的 连 杆 将 被 建 模 为 凸 形 或 复合 凸 形 。 我 们 现在 选 
择 第 一 个 移动 连 杆 ( 即 对 象 robot_link1) ,并 通过 依次 单 击 菜单 栏 、 添 加 、 选 择 的 凸 包 以 生成 
一 个 凸 形 。 将 其 重 命名 为 robot_link_dyn1, 并 将 其 分 配给 可 见 层 9。 如 果 提 取 凸 包 时 不 能 
保留 原始 形状 的 足够 细节 ,你 仍然 可 以 从 其 组 成 元 素 中 手动 提取 几 个 凸 包 ,然后 通过 依次 单 
击 菜单 栏 编辑 、 分 组 /合并 、 分 组 所 选 形 状 以 将 所 有 凸 包 分 组 。 如果 这 种 方法 有 问题 或 耗 
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时 ,那么 你 可 以 通过 依次 单 击 菜单 栏 添 加、 选择 的 凸 形 分 解 以 自动 提取 凸 分 解 形状 ,得 到 的 
模型 如 图 3-113 所 示 。 


图 3-113 原形 形状 进行 分 解 后 得 到 的 凸 形 


现在 对 所 有 剩余 的 机 器 人 连 杆 重复 相同 的 过 程 。 一 旦 完成 ,我 们 将 每 个 可 见 形状 附加 
到 其 相应 的 不 可 见 的 动态 垂 饰 。 首 先 选择 可 见 形状 ,然后 通过 控制 单 击 选择 其 动态 垂 饰 , 然 
后 依次 选择 菜单 栏 编 辑 , 最 后 选择 的 父 对 象 。 如 图 3-114 所 示 ,将 可 见 形状 拖 动 到 场景 层 
次 结构 中 的 动态 垂 饰 上 也 可 以 实现 相同 的 结果 。 
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图 3-114 可 见 形状 附加 到 它们 的 动态 形状 


我 们 仍然 需要 处 理 几 件 事情 : 

(1) 因为 我 们 希望 动态 形状 只 对 物理 引擎 可 见 , 而 不 是 对 其 他 计算 模块 可 见 , 所 以 我 们 
在 对 象 公 共 属 性 中 取消 选中 动态 形状 的 所 有 对 象 特殊 属性 。 

(2) 我 们 仍然 必须 将 动态 形状 配置 为 动态 的 和 可 响应 的 ,可 以 在 形状 动力 学 属性 中 做 
到 这 一 点 。 首 先 选择 基本 动态 形状 ( 即 robot_dyn) ,然后 检查 Body 是 否 是 可 响应 项 。 启 用 
前 4 个 局 部 可 响应 掩 码 标志 ,并 禁用 最 后 4 个 局 部 可 响应 掩 码 标志 : 这 对 于 连续 的 可 响应 
连 杆 不 会 彼此 冲突 来 说 是 很 重要 的 。 对 于 机 器 人 中 的 第 一 个 移动 动态 连 杆 ( 即 robot link | 
dyn1) ,我 们 还 启用 了 Body 可 响应 项 ,但 是 这 次 我 们 禁用 前 4 个 本 地 可 响应 掩 码 标 志 , 并 启 
用 最 后 4 个 局 部 可 响应 掩 码 标志 。 对 所 有 其 他 动态 连 杆 都 重复 上 述 过 程 ,同时 总 是 交替 局 
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部 可 应 答 掩 码 标志 : 一 旦 模型 被 定义 ,机 器 人 的 连续 动态 形状 在 彼此 交互 时 就 不 会 产生 任 
何冲 突 响 应 。 尝 试 以 机 器 人 动态 基 座 所 在 的 构造 物 结束 ,并 且 机 器 人 的 最 后 一 个 动态 连 杆 
只 启用 4 个 局 部 响应 标志 ,以 便 我 们 将 机 器 人 连接 到 移动 平台 ,或 附加 夹 持 器 到 机 器 人 的 最 
后 动态 连 杆 ,同时 保证 没有 动态 碰撞 干扰 。 

(3) 我 们 仍然 需要 标记 动态 形状 ,因为 Body 是 动态 的 ,也 可 以 在 形状 动力 学 属性 中 做 
到 这 一 点 。 然 后 ,我 们 可 以 手动 输入 质量 和 惯性 张 量 属性 ,或 通过 单 击 计算 选 定 凸 形 的 质量 
和 惯性 属性 。 机 器 人 的 动态 基 座 是 一 种 特殊 情况 : 大 多 数 时 候 ,我 们 希望 机 器 人 的 基 座 ( 即 
robot_dyn) 是 非 动态 的 ( 即 静 态 的 ) ,否则 ,机 器 人 可 能 在 运动 期 间 下 落 。 但 是 ,一 旦 将 机 器 
人 的 基 座 连接 到 移动 平台 ,我 们 希望 基 座 变 为 动态 ( 即 非 静态 ) 。 为 此 ,我 们 可 以 启用 "Set to 
dynamic if gets parent” 选 项 ,然后 关闭 "Body is dynamic item” 选 项 。 现 在 运行 仿真 : 所 有 动态 
形状 ,除了 机 器 人 的 基础 ,按理 说 是 会 下 落 的 。 附 加 的 视觉 形状 将 遵循 它们 的 动态 垂 饰 。 

4. 模型 定义 

现在 准备 好 定义 我 们 的 模型 。 通 过 构建 模型 组 合 开始 : 选择 robot_link_dyn6, 然 后 控 
制 选择 robot_joint6, 并 通过 依次 单 击 菜单 栏 、 编 辑 、 后 


最 后 选择 的 父 对 象 ,将 最 后 一 个 动态 机 器 人 连 杆 ET 
(robot link dyn6) 附加 到 其 对 应 的 关节 。 也 可 以 通 B 9; pia 
过 将 对 象 robot link dyn6 拖 放 到 场景 层次 结构 中 的 E- É robot joint 
robot link6 上 来 完成 此 步骤 。 现在 继续 将 robot_ ES 
joint6 和 robot_link_dyn5 组 合 起 来 , 依 此 类 推 ,直到 gs r jer: Apen A 
到 达 机 器 人 的 基地 。 如 图 3-115 所 示 , 现 在 有 以 下 场 Lact 
景 层次 结构 。 eT O sean 

模型 基 座 最 好 有 一 个 简单 的 名 称 , 因 为 模型 基 座 "i arg e m 
往往 也 代表 模型 本 身 。 因 此 ,将 robot 重 命名 为 ls vesti 
robot_visibleBase, 将 robot dyn 重 命名 为 robots W By @ robot link, dynt 
在 我 们 选择 层次 树 的 基础 ( 即 对 象 机 器 人 ) ,在 对 象 公 一 一 一 
共 属 性 中 启用 Object is model base. 同时 启用 图 3-115 机 器 人 模型 层次 
Object/model can transfer or accept DNA。 模 型 边 
界 框 将 出 现 , 并 包围 整个 机 器 人 。 然 而 ,边界 框 看 起 来 太 大 ,这 是 因为 边界 框 还 包含 不 可 见 


项 目 (例如 关节 )。 现 在 ,通过 为 所 有 关节 启用 Don't show as inside model selection, 从 模型 
边界 框 中 排除 关节 。 我 们 可 以 对 模型 中 的 所 有 不 可 见 项 目 执行 相同 的 过 程 。 这 也 是 一 个 从 
模型 边界 框 中 排除 大 传感器 或 其 他 项 目的 方法 ,如 图 3-116 所 示 。 

接 下 来 需要 保护 我 们 的 模型 免 受 意外 修改 。 我 们 选择 机 器 人 中 的 所 有 可 见 对 象 , 然 后 
启用 Select base of model instead; 如 果 我 们 现在 单 击 场景 中 的 可 见 关 节 , 将 会 选择 到 机 器 
人 的 基 座 ,这 允许 我 们 像 操 作 一 个 单一 对 象 一 样 操纵 模型 。 还 可 以 通过 在 场景 中 的 同时 单 
击 Shift 和 Ctrl 键 或 通过 选择 场景 层次 结构 中 的 对 象 来 选择 机 器 人 中 的 可 见 对 象 。 

接 下 来 将 机 器 人 置 于 正确 的 默认 位 置 /方向 。 首 先 ,保存 当前 场景 作为 参考 (例如 ,如 果 
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图 3-116 机 器 人 模型 边界 框 


在 稍 后 阶段 ,我 们 需要 导入 具有 相同 方向 的 CAD 数据 )。 然 后 选择 模型 并 适当 地 修改 其 位 
置 /方向 。 最 好 的 做 法 是 将 模型 ( 即 其 基 座 对 象 ) 定 位 在 X — 0 和 Y — 0, 如 图 3-117 所 示 。 


图 3-117 在 默认 配置 情况 下 的 机 器 人 模型 


运行 仿真 时 ,机 器 人 将 崩溃 ,因为 关节 不 是 默认 控制 的 。 在 前 一 阶段 添加 关节 时 ,我 们 
是 在 力 /力矩 模式 下 创建 关节 ,但 是 它们 的 电机 或 控制 器 被 禁用 (默认 )。 现 在 可 以 根据 我 们 
的 要 求 调整 关节 。 在 该 例 中 ,我 们 想 要 一 个 简单 的 PID 控制 器 。 在 关节 动态 属性 中 , 单 击 
电机 启用 ,并 调整 最 大 力矩 。 然 后 , 单 击 控制 回路 启用 并 选择 位 置 控制 (PID)。 我 们 现在 再 
次 运行 仿真 ,机 器 人 将 会 保持 其 位 置 。 尝 试 切换 当前 物理 引擎 ,以 查看 所 有 支持 的 物理 引擎 
的 行为 是 否 一 致 。 你 可 以 通过 相应 的 工具 栏 按钮 .或 在 一 般 动力 学 属性 中 做 到 这 一 点 。 

在 仿真 期 间 , 通 过 动态 内 容 可 视 化 和 验证 工具 栏 按 钮 验证 场景 动态 内 容 。 现 在 ,只 有 物 
理 引擎 考虑 的 项 目 将 被 显示 .并且 显示 是 彩色 编码 的 。 这 样 做 是 非常 重要 的 ,特别 是 当 你 的 
动态 模型 不 按 预期 的 行为 时 ,可 以 快速 调试 模型 。 类 似 地 ,在 仿真 期 间 始 终 查 看 场景 层次 结 
构 : 动态 启用 的 对 象 应 在 其 名 称 的 右 侧 显示 球形 边界 图 标 , 如 图 3-118 所 示 。 

最 后 ,我 们 需要 准备 好 机 器 人 ,以 便 我 们 能 够 轻松 地 将 夹具 连接 到 机 器 人 ,或 者 容易 地 
将 机 器 人 附 接 到 移动 平台 。 两 个 动态 启用 的 形状 可 以 以 两 种 不 同 的 方式 彼此 刚性 地 附 接 : 
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图 3-118 动态 内 容 可 视 化 和 验证 


CD 通过 分 组 : 选择 形状 ,然后 依次 单 击 菜单 栏 . 编 辑 . 分 组 /合并 、 分 组 选 定 的 形状 。 

(2) 通过 力 /力矩 传感器 连接 它们 : 力矩 传感器 还 可 以 用 作 两 个 单独 的 动态 使 能 形状 
之 间 的 刚性 连 杆 。 

在 我 们 的 例子 中 只 关注 第 2 种 方式 。 我 们 通过 依次 单 击 菜单 栏 .添加 力 传感器 以 创建 
力 / 力 矩 传感器 ,然后 将 其 移动 到 机 器 人 的 顶端 ,然后 将 其 附加 到 对 象 robot link dyn6, 3& 
当地 改变 其 尺寸 和 视觉 外 观 ( 红 色 力 /力矩 传感器 通常 被 认为 是 可 选 的 附 接 点 ,请 检查 各 种 
机 器 人 是 否 可 用 ) ,将 其 名 称 更 改 为 robot_attachment, 如 图 3-119 所 示 。 
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图 3-119 ”附件 力 /力矩 传感器 
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现在 我 们 将 一 个 夹具 模型 拖 到 场景 中 ,保持 选择 它 , 然 后 按 住 Ctrl 键 并 单 击 附 件 力 传 
感 器 ,然后 单 击 组 装 /拆卸 工具 栏 按钮 。 夹 具 将 会 附加 上 去 ,如 图 3-120 所 示 。 


图 3-120 ”附加 夹具 


因为 夹具 在 模型 定义 期 间 被 适当 地 配置 , 它 会 准确 附加 上 去 。 我 们 现在 还 需要 正确 配 
置 机 器 人 模型 ,以 便 知 道 它 如 何 将 自身 附加 到 移动 基础 上 。 选 择机 器 人 模型 ,然后 在 对 象 常 
用 属性 中 单 击 组 合 。 禁 用 对 象 可 以 具有 ”* 父 ”角色 ,然后 单 击 设置 矩阵 。 这 将 记忆 当前 基础 
对 象 的 局 部 变换 矩阵 ,并 使 用 它 来 相对 于 移动 机 器 人 的 附 接点 定位 /定向 。 为 了 验证 我 们 的 
正确 性 , 拖 动 模型 Models/robots/mobile/KUKA Omnirob. ttm 到 场景 中 。 然 后 选择 我 们 
的 机 器 人 模型 , 按 住 Ctrl 键 并 单 击 移动 平台 上 的 一 个 连接 点 ,然后 单 击 组 装 / 拆 务 工具 栏 
按钮 。 

现在 可 以 向 机 器 人 添加 额外 的 项 目 . 例 如 传感器 。 在 某 些 时 候 , 我 们 可 能 还 想 要 将 嵌入 
式 脚本 附加 到 模型 中 ,以便 控制 其 行为 或 为 各 种 目的 配置 它 。 在 这 种 情况 下 ,请 确保 已 经 了 
解 如 何 从 咀 入 式 脚 本 访问 对 象 句柄 。 我 们 还 可 以 从 插件 、 从 远程 API 客户 端 \ 从 ROS 节点 
或 从 附加 组 件 控制 /访问 /接口 模型 。 

现在 确保 我 们 已 经 还 原 机 器 人 和 夹具 附件 中 的 更 改 , 折 释 机 器 人 模型 的 层次 结构 树 , 选 
择 模型 的 基 座 ,然后 依次 单 击 菜单 栏 文件 ,保存 模型 为 。 如 果 我 们 将 它 保存 在 模型 文件 夹 
中 ,那么 模型 将 在 模型 浏览 器 中 可 用 。 

3.10.2 WZA 

本 节 将 尝试 解释 如 何 使 用 逆 运 动 学 功能 ,同时 构建 一 个 7DoF 元 余 机 械 手 。 但 在 此 之 
前 ,请 确保 查看 文件 夹 scenes/ik fk simple examples 中 与 IK 和 FK 相关 的 各 种 简单 示例 
场景 。 我 们 将 构建 一 个 非 动态 机 械 臂 , 它 只 使 用 逆 运 动 学 ,而 不 使 用 任何 物理 引擎 功能 。 与 
本 部 分 内 容 相关 的 V-REP CAD 数据 (“redundantManipulator. stl 位 于 V-REP 安装 文件 
夹 的 cadFiles 文件 夹 中 。 与 本 部 分 内 容 相关 的 V-REP 场景 可 以 在 V-REP 的 安装 文件 夹 的 
tutorials\InverseKinematics 文件 夹 中 找到 。 依 次 单 击 菜单 栏 .文件 .导入 .网 格 ,然后 选择 
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要 导入 的 文件 。 此 时 将 弹出 一 个 对 话 框 ,询问 网 格 缩放 和 网 格 方向 。 单 击 确定 按钮 将 导入 
位 于 场景 中 间 的 一 个 简单 的 形状 。 形状 也 会 出 现在 主 窗口 左 侧 的 场景 层次 结构 中 。 根 据 原 
始 CAD 数据 的 导出 方式 ,导入 的 CAD 数据 可 能 处 于 不 同 的 比例 .不同 的 位 置 ,或 甚至 细 分 
为 多 个 形状 。 导 入 的 图 形 的 分 配 颜 色 是 随机 的 。 如 图 3-121 所 示 , 它 显示 了 导入 的 形状 。 


图 3-121 导入 的 模型 形状 


原始 形状 被 分 成 几 个 子 形状 (也 可 参见 场景 层次 结构 )。 形 状 分 割 算法 通过 对 公共 边缘 
连 杆 的 所 有 三 角形 进行 分 组 来 进行 操作 。 根 据 原始 网 格 的 创建 或 导出 方式 ,无 法 执行 此 类 
划分 过 程 。 在 这 种 情况 下 ,你 将 不 得 不 在 三 角形 编辑 模式 中 手动 提取 形状 。 

接 下 来 ,将 改变 各 种 对 象 的 颜色 ,以 便 使 其 具有 良好 的 视觉 外 观 。 首 先 双击 场景 层次 结 
构 中 的 形状 图 标 ,打开 形状 属性 对 话 框 。 在 选择 形状 时 , 单 击 对 话 框 中 的 调整 外 部 颜色 : 这 
将 允许 你 调整 所 选 形 状 的 外 部 面 的 各 种 颜色 分 量 。 现 在 ,只 需 调整 形状 的 环境 / 漫 反 射 颜色 
分 量 。 若 要 将 一 种 形状 的 颜色 转换 为 另 一 种 形状 ,请 选择 这 两 种 形状 ,并 确保 最 后 一 个 选 定 
的 形状 (用 白色 边框 指示 ) 是 你 要 采用 颜色 的 形状 ,然后 单 击 在 形状 对 话 框 的 “颜色 ”部 分 的 
“应 用 于 选择 ?按钮 。 也 可 以 随意 调整 其 他 视觉 参数 ,如 阴影 角度 参数 .边缘 宽度 或 边缘 颜 
色 。 一 旦 完成 了 着 色 ,你 可 能 得 到 如 图 3-122 所 示 的 场景 。 

在 下 一 步 中 ,我 们 将 添加 机 械 臂 的 7 个 关节 。 一 种 方法 是 将 关节 添加 到 场景 中 ,然后 指 
定 它们 适当 的 位 置 和 方向 (通过 坐标 和 变换 对 话 框 )。 然 而 : 当 你 不 知道 确切 的 关节 位 置 ,这 
是 不 可 能 做 到 的 ,所 以 必须 从 我 们 有 的 形状 提取 : 选择 所 有 导入 的 形状 ,然后 依次 单 击 菜单 
栏 编辑 .边界 框 对 齐 .将 所 选 形状 的 坐标 框架 与 世界 对 齐 。 此 操作 确保 我 们 的 边界 框 与 绝 
对 参考 框架 对 齐 ,并 且 给 定 的 当前 机 械 臂 配置 表示 了 最 小 的 边界 框 。 依 次 单 击 菜单 栏 、 添 
加 、 关 节 、 旋 转 ,将 旋转 关节 插入 场景 。 默 认 位 置 为 (0; 0; 0) ,其 默认 方向 为 垂直 ,因此 关节 
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图 3-122. 着 色 后 的 模型 外 观 


被 机 械 手 的 基准 圆柱 体 隐藏 。 当 关节 仍然 被 选中 时 , 按 下 Ctrl 键 选择 基 圆柱 ,然后 打开 位 
置 和 平移 对 话 框 , 并 单 击 第 1 部 分 底部 的 “应 用 于 选择 "。 这 只 是 将 关节 定位 在 与 基 圆柱 完 
全 相同 的 坐标 系 下 (这 个 操作 知识 轻微 地 调整 了 关节 的 垂直 位 置 ,因为 它 已 经 在 恰当 的 位 置 
了 ) 。 现 在 为 机 械 臂 的 所 有 关节 重复 这 个 过 程 ( 记 住 总 共有 7 个 关节 )。 所 有 的 关节 现在 就 
位 ,然而 ,其 中 一 些 方向 有 错误 。 选 择 与 世界 Y 轴 对 齐 的 所 有 关节 ,然后 在 方向 和 旋转 对 话 
框 的 第 1 节 中 为 Alpha, Beta 和 Gamma 项 输入 (90,0,0) ,然后 单 击 相关 的 “应 用 于 选择 ” 按 
钮 。 接 下 来 ,选择 与 世界 X 轴 对 齐 的 关节 ,然后 为 Alpha、Beta fll Gamma 输入 (0,90,0)。 
现在 ,所 有 关节 都 有 正确 的 位 置 和 方向 。 

现在 可 以 在 关节 属性 对 话 框 中 调整 关节 尺寸 (检查 关节 长 度 和 关节 直径 项 目 ) (也 可 以 
通过 双击 场景 层次 结构 中 的 关节 图 标 打开 )。 确 保 所 有 关节 清晰 可 见 ,如 图 3-123 所 示 。 

下 一 步 将 对 属于 同一 刚体 的 形状 进行 分 组 。 选 择 作为 连 杆 1( 基 本 圆柱 为 “ 连 杆 0”) 一 
部 分 的 5 个 形状 ,然后 依次 单 击 菜单 栏 编 辑 、 分 组 /合并 、 分 组 所 选 形状 。 一 旦 形状 以 复合 
形状 分 组 ,就 可 以 重新 将 其 边界 框 与 世界 对 齐 . 但 此 步 又 不 是 必需 的 (并 且 只 有 视觉 效果 )。 
对 人 逻辑 上 属于 一 体 的 所 有 形状 重复 相同 的 过 程 。 在 本 部 分 内 容 中 ,我 们 不 会 启动 夹具 的 手 
指 ,因此 只 是 简单 地 将 它们 与 最 后 一 个 关节 分 组 。 当 所 有 要 分 组 的 形状 共享 相同 的 视觉 属 
性 时 ,尝试 将 它们 合并 在 一 起 (依次 单 击 菜单 栏 编辑 ,分 组 /合并 ,合并 所 选 形状 )。 

此 时 ,可 以 按照 以 下 方式 重 命名 场景 中 的 所 有 对 象 : 依次 单 击 redundantRobot、 
redundantRob_jointl .redundantRob_linkl ,redundantRob_joint2 等 。 只 需 双 击 场景 中 的 层 
次 结构 中 的 对 象 名 称 即 可 编辑 其 名 称 。 

现在 我 们 可 以 建立 运动 链 , 从 顶端 到 基 座 : 选择 对 象 "redundantRob_link7”, 然 后 按 下 
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3-123 ”添加 关节 后 的 模型 


Ctrl 键 选择 对 象 *redundantRob_joint7”, 然 后 依次 单 击 菜单 栏 , 编 辑 、 最 后 选择 的 对 象 父 。 
或 者 ,你 可 以 将 对 象 拖 动 到 场景 层次 结构 中 的 另 一 个 对 象 上 ,以 实现 类 似 的 操作 。 接 下 来 对 
对 象 “redundantRob_joint7” 和 对 象 “*redundantRob_link6” 执 行 相 同 操 作 。 以 相同 的 方式 继 
续 , 直 到 建立 了 机 械 臂 的 整个 运动 链 。 这 时 你 应 该 (注意 场景 层次 结构 的 结构 ) : 

选择 所 有 关节 ,然后 在 关节 对 话 框 中 ,在 “关节 模式 部 分 中 选择 “关节 处 于 逆 运 动 学 模 
式 ”, 然 后 单 击 “ 应 用 于 选择 "按钮 。 打 开 对 象 公共 属性 ,在 可 见 性 图 层 部 分 ,禁用 图 层 2 并 启 
用 图 层 10 ,然后 单 击 相关 的 “应 用 于 选择 按钮 。 这 只 是 将 所 有 关节 发 送 到 可 见 层 10, 有 效 
地 使 它们 不 可 见 。 如 果 你 希望 临时 启用 /禁用 某 些 图 层 ,请 查看 图 层 选择 对 话 框 。 

现在 将 为 机 械 臂 定义 一 个 逆 运 动 学 任务 。 在 V-REP 中 ,IK 任务 需要 至 少 规定 以 下 
元 素 : 

@ 一 个 用 "tip”dummy 和 “base” 对 象 描述 的 运动 链 。 

© 一 个 约束 "tip" dummy 的 "target" dummy. 

我 们 已 经 有 了 “base” 对 象 (对 象 “redundantRobot”)。 再 添加 一 个 dummy 对 象 , 将 其 重 
命名 为 “redundantRob_tip”, 并 使 用 坐标 和 变换 对 话 框 将 其 位 置 设置 为 (0. 324,0,0. 62)。 
接 下 来 ,将 dummy 连接 到 “redundantRob_link7” (选择 redundantRob_tip”, 然 后 选择 
“redundantRob_link7”, 然 后 依次 单 击 Menu bar, Edit, Make last selected object parent)。 
这 样 ,"tip”dummy 就 准备 好 了 。 

现在 让 我 们 准备 "target" dummy: 复制 和 粘贴 "redundantRob_tip”, 并 将 副本 重 命名 为 
*redundantRob _target”。" target" dummy 准备 就 绪 。 接 下 来 .我 们 必须 通知 V-REP 
“redundantRob_tip” 和 "redundantRob_target” 是 用 于 逆 运 动 学 分 辩 率 的 一 个 tip-target 对 。 
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双击 场景 层次 结构 中 的 "redundantRob_tip "虚拟 图 标 , 这 将 打开 虚拟 属性 对 话 框 。 在 
Dummy-dummy 连接 部 分 中 ,将 "redundantRob_target”" 指 定 为 Linked dummy。 注 意 两 个 
虚拟 变量 通过 场景 层次 结构 中 的 红色 刻 线 连接 起 来 (两 个 虚拟 变量 也 通过 红线 在 场景 中 连 
杆 , 但 由 于 两 个 虚拟 变量 是 重合 的 ,因此 无 法 看 到 该 行 )。 在 同一 对 话 框 中 , 连 杆 类 型 已 经 是 
IK,tip-target, 这 是 默认 值 。 这 时 你 现在 应 该 能 得 到 如 图 3-124 所 示 的 场景 。 
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图 3-124 添加 了 IK 组 的 模型 


在 这 个 阶段 ,用 于 定义 逆 运 动 学 任务 的 所 有 元 素 都 准备 就 绪 , 我 们 只 需要 将 任务 注册 为 
IK 组 。 打 开道 运动 学 对 话 框 ,然后 单 击 添加 新 TK 组 。IK 组 列表 中 将 显示 一 个 新 项 目 一 一 
“IK_Group”。 选 择 该 项 目 后 , 单 击 编辑 IK 元 素 以 打开 IK 元 素 对 话 框 。 在 Add new IK 
element with tip 按钮 旁边 ,在 下 拉 框 中 选择 redundantRob_tip”, 然 后 单 击 Add new IK 
element with tip 按钮 。 这 只 是 添加 了 一 个 显示 在 列表 中 的 IK 元 素 。 接 下 来 ,指示 
“redundantRobot” 作 为 “Base”。 最 后 ,确保 在 "Constraints” 部 分 中 检查 所 有 项 目 ( 检 查 als 
Alpha-Beta 和 Gamma)。 事 实 上 ,如 图 3-125 所 示 ,我 们 希望 "tip”dummy 在 位 置 和 方向 上 
跟随 "target" dummy. 

关闭 IK 元 素 对 话 框 。 在 道 运 动 学 对 话 框 中 ,可 以 自由 地 检查 选项 *Mechanism is 
redundant”, 但 在 此 阶段 , 它 不 会 有 任何 差异 .因为 没有 定义 关节 限制 或 障碍 物 回避 参数 。 

我 们 的 逆 运 动 学 任务 已 经 准备 好 了 。 运 行 仿真 ,然后 选择 “元 余 Rob target”, FE, 
选择 对 象 平移 工具 栏 按钮 。 现 在 用 鼠标 拖 动 对 象 : 机 械 臂 应 该 会 跟随 。 也 可 以 尝试 对 象 旋 
转 工具 栏 按钮 。 

尝试 在 操作 过 程 中 按 住 Ctrl 或 Shift 键 。 切 换 回 对 象 平移 工具 栏 按钮 ,并 尝试 尽 可 能 
远 地 拖 动 对 象 ,并 注意 逆 运 动 学 任务 如 何 中 断 。 实 际 上 , 当 配 置 是 奇异 或 不 可 达 时 ,会 发 生 
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图 3-125 IK 元 素 对 话 框 


这 种 情况 ,但 是 这 种 行为 有 解决 方法 : 当 仿 真 仍 在 运行 时 ,在 逆 运 动 学 对 话 框 列表 中 选择 
“IK_Group”, 然 后 为 Calc。 Method 项 指定 DLS。 将 对 象 拖 动 到 不 能 触及 的 地 方 ,注意 逆 运 
动 学 解 如 何 变 得 更 稳定 。 尝 试 上 下 调整 Damping 项 。 基 本 上 , 当 阻 尼 大 时 , 解 会 变 得 更 稳 
XE ,但 更 慢 。 实 际 上 ,你 可 以 获得 两 种 求解 方法 的 优点 ,你 需要 做 的 是 定义 两 个 相同 的 “IK 
组 ”, 其 中 第 一 个 为 非 阻 尼 , 第 二 个 为 阻尼 。 然 后 ,对 于 第 二 个 “IK 组 ”, 你 可 以 指定 一 个 条 件 
解 。 现 在 将 Calce，method” 设 回 “Pseudo inverse". 

现在 ,在 IK 元 素 对 话 框 中 ,选择 “redundantRob_tip”, 然 后 尝试 禁用 一 些 约束 项 ,并 注 
意 当 “redundantRob_target” 对 象 被 拖 动 或 旋转 时 机 械 手 的 行为 。 一 旦 实验 完成 ,将 所 有 约 
柬 项 目 重 置 为 “已 选中 ”, 然 后 停止 仿真 。 

我 们 现在 要 做 的 是 添加 一 种 方式 来 轻松 地 操纵 机 器 人 ,而 不 必 担 心 由 于 移动 错误 的 对 
象 而 破坏 机 器 人 。 因 此 ,我 们 将 其 定义 为 一 个 模型 。 首 先 ,将 “redundantRob_tip” 和 
“redundantRob_target” 移 动 到 第 11 层 ,使 两 个 虚拟 变量 不 可 见 。 然 后 Shift 选择 场景 视图 
中 的 所 有 可 见 对 象 ,Ctrl 单 击 场景 层次 结构 中 的 对 象 ~*redundantRobot” 以 将 其 从 选择 中 删 
除 , 然 后 打开 对 象 公 共 属 性 对 话 框 。 检 查 选 择 模 型 的 基础 项 目 , 然 后 选择 相关 的 “Apply to 
selection” 按 钮 。 使 用 Esc 键 清除 选择 ,然后 选择 “redundantRobot”。 在 同一 对 话 框 中 , 检 
查 对 象 是 模型 基础 项 ,然后 关闭 对 话 框 。 如 图 3-126 所 示 ,边界 框 现在 包含 整个 机 械 臂 。 

单 击 机 械 臂 上 的 任何 对 象 .并 注意 base dummy(“redundantRobot”) 始 终 被 选中 。 现 在 
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图 3-126 边界 框 包 含 了 整个 机 械 臂 模型 


打开 对 象 操作 设置 并 检查 它 ( 确 保 选择 “redundantRobot”)。 你 可 以 看 到 “redundantRobot” 
的 平移 发 生 在 绝对 参考 系 的 y 平面 中 。 你 可 以 通过 调整 各 个 项 目 来 更 改 所 选 对 象 的 默认 
鼠标 平移 行为 。 

现在 当 你 尝试 在 场景 中 移动 或 旋转 机 器 人 (使 用 对 象 操纵 工具 栏 按钮 ) 时 , 它 将 始终 固 
定 在 地 板 上 ,并 保持 正确 的 方向 。 你 可 以 尝试 一 下 (然后 确保 你 使 用 撤销 工具 栏 按钮 将 其 重 
置 为 其 初始 位 置 /方向 )。 如 果 在 平移 操作 期 间 按 Ctrl 键 , 则 可 以 上 下 移动 机 器 人 。 

接 下 来 ,让 我 们 添加 一 个 “manipulation sphere” 的 球体 ,我 们 将 使 用 它 来 操纵 机 器 人 的 
夹具 位 置 /方向 。 依 次 单 击 菜单 栏 ` 添 加 、 基 本 形状 .球体 打开 基本 形状 对 话 框 ,将 X 尺寸 ,Y 
尺寸 和 Z 尺寸 指定 为 0.05, 然 后 取消 选中 创建 动态 和 可 响应 形状 项 并 单 击 “ 确 定 按 钮 。 将 
新 添加 的 球体 位 置 调整 为 与 redundantRob_target” 相 同 ( 使 用 坐标 和 变换 对 话 框 )。 球 体 
现在 会 出 现在 机 械 手 的 末端 。 将 球体 重 命名 为 “redundantRob_manipSphere”, 然 后 将 其 作 
为 "redundantRob_target "的 父 级 。 当 你 运行 仿真 时 ,你 应 该 能 够 通过 移动 操作 球体 来 更 改 
机 械 臂 的 配置 。 再 次 停止 仿真 。 

让 我 们 更 改 一 些 其 他 细节 。 在 形状 属性 对 话 框 中 , 单 击 调整 外 部 颜色 ,然后 检查 不 透明 
度 项 。 注 意 球体 外 观 的 变化 。 为 了 得 到 更 好 的 外 观 , 请 检查 形状 对 话 框 中 的 背面 拣选 项 目 。 
在 对 象 公共 属性 对 话 框 中 取消 选中 对 象 特殊 属性 部 分 中 的 所 有 项 (这 是 因为 操纵 球 不 真 的 
属于 机 械 臂 , 它 更 多 是 一 个 用 户 界面 元 素 )。 如 图 3-127 所 示 ,现在 使 "redundantRobot” 
parent 为 “redundantRob_manipSphere”。 

作为 最 后 一 步 .我 们 将 注册 一 个 碰撞 对 象 . 它 会 检测 机 械 辟 与 其 环境 之 间 的 碰撞 。 我 们 
想 要 的 是 机 械 辟 中 的 每 个 单独 形状 (操纵 球 除外 ) 能 够 检测 与 环境 的 碰撞 。 让 我 们 首先 为 我 
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图 3-127 添加 了 球体 的 机 械 臂 模型 


们 的 机 械 辟 定义 一 个 集合 。 依 次 单 击 菜单 栏 工具、 集合 或 单 击 相 应 的 工具 栏 按 钮 ,打开 集 
合 对 话 框 。 选 择 “redundantRobot”, 然 后 单 击 * 添 加 新 集合 ”。 添 加 了 一 个 新 的 空 集合 。 我 
们 现在 需要 定义 集合 内 容 : 单 击 添加 (确保 “redundantRobot "仍然 被 选中 ) 。 注 意 集 合 的 内 
容 如 何 更 改 。 现 在 选择 新 添加 的 集合 项 ,然后 单 击 可 视 化 所 选集 合 : 构成 机 械 臂 的 所 有 对 
象 在 场景 中 获得 粉红 色 的 颜色 ( 注 : 即 图 3-128 右 侧 的 深 色 部 分 )。 将 集合 重 命名 为 
“redundantRob”, 这 时 将 得 到 如 图 3-128 所 示 的 场景 。 

现在 定义 了 ”redundantRob” 集 合 ,我 们 可 以 注册 一 个 碰撞 对 象 : 打开 碰撞 对 话 框 ,然后 
单 击 添加 新 碰撞 对 象 并 指定 以 下 项 对 :“[Collection] redundantRob” 场 景 中 的 所 有 其 他 可 
碰撞 对 象 "。 这 添加 了 一 个 新 的 冲突 对 象 ,你 可 以 通过 双击 (将 其 重 命名 为 “redundantRob”) 在 
列表 中 重 命名 。 

折 释 场景 层次 结构 中 的 “redundantRobot" 树 。 现 在 你 的 宛 余 机 械 臂 模型 已 准备 就 绪 。 
运行 仿真 ,并 复制 .粘贴 几 次 机 械 臂 。 移 动 /旋转 副本 ,并 通过 拖 动 其 操作 球体 来 更 改 其 配 
置 。 注 意 每 个 机 器 人 实例 是 如 何 完全 功能 的 ,以 及 如 何 用 颜色 变化 来 指示 碰撞 。 打 开 逆 运 
动 学 对 话 框 ,收集 对 话 框 和 碰撞 检测 对 话 框 。 请 注意 所 列 项 目 是 如 何 被 自动 复制 的 。 现 在 
停止 仿真 。 注 册 最 小 距离 对 象 的 过 程 与 上 面 的 碰撞 对 象 注册 非常 相似 。 所 有 已 注册 的 对 象 
《碰撞 检 测 、 集 合 、IK 组 等 ): 和 所 有 场景 对 象 都 可 以 通过 相应 的 API 调用 访问 。 此 外 ,它们 
可 以 直接 记录 并 通过 图 形 对 象 可 视 化 。 
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图 3-128 定义 了 集合 的 机 械 臂 模型 


3.10.3 V-REP 与 MATLAB 连接 的 例子 


1. 读 取 V-REP 中 机 器 人 模型 的 关节 角 

在 V-REP 的 场景 中 ,添加 一 个 Baxter 机 器 人 。 然 后 在 MATLAB 中 保存 下 面 的 函数 。 
先 运行 V-REP, 再 运行 下 面 的 函数 , 即 可 在 Baxter 机 器 人 运行 的 过 程 中 , 读 取 每 一 个 时 刻 
的 关节 角 ,并 保存 为 , txt 的 文本 格式 。 

MATLAB 函数 如 下 : 


function baxter() 
disp('Program started'); 
* Vrep = remApi('remoteApi', 'extApi.h'); % using the header (requires a compiler) 
vrep- remApi('remoteApi'); % using the prototype file (remoteApiProto. m) 
vrep.simxFinish( —1); % just in case, close all opened connections 
clientID = vrep. simxStart('127.0.0.1', 19999, true, true, 5000,5); 
rli=[]; 
r2=[]; 
r3=[]; 
r4=[]; 
r5=[]; 
r6=[]; 
r7=[]; 


if (clientID>- 1) 
disp('Connected to remote API server'); 
% get handle for Baxter rightArm jointl 
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[res, handle rigArmjointl] = vrep. simxGetObjectHandle(clientID, 'Baxter rightArm | 
jointl',vrep.simx opmode oneshot wait); 
[res, handle rigArmjoint2] = vrep. simxGetObjectHandle(clientID, 'Baxter rightArm | 
joint2',vrep.simx opmode oneshot wait); 
[res, handle rigArmjoint3] = vrep. simxGetObjectHandle(clientID, 'Baxter rightArm _ 
joint3',vrep.simx opmode oneshot wait); 
[res, handle rigArmjoint4] = vrep. simxGetObjectHandle(clientID, 'Baxter rightArm 
joint4',vrep.simx opmode oneshot wait); 
[res, handle rigArmjoint5] = vrep. simxGetObjectHandle(clientID, 'Baxter rightArm | 
joint5',vrep.simx opmode oneshot wait); 
[res, handle rigArmjoint6] = vrep. simxGetObjectHandle(clientID, 'Baxter rightArm 
joint6',vrep.simx opmode oneshot wait); 
[res, handle rigArmjoint7] = vrep. simxGetObjectHandle(clientID, 'Baxter rightArm | 
joint7',vrep.simx opmode oneshot wait); 
while(vrep. simxGetConnectionId(clientID) 一 = - 1), % while v- rep connection is 
stillactive 
t = vrep.simxGetLastCmdTime(clientID)/1000.0; % get current simulation time 
if (t > 1000) break; 
end & stop after t = 1000 seconds 
[res,rlangle] = vrep. simxGetJointPosition(clientID, handle rigArmjointl, vrep. simx opmode _ 
oneshot wait); 


[res, r2angle] = vrep. simxGetJointPosition(clientlID, handle rigArmjoint2, vrep. simx opmode _ 
oneshot wait); 


[res,r3angle] = vrep. simxGetJointPosition(clientlID, handle rigArmjoint3, vrep. simx opmode _ 
oneshot wait); 


[res,r4angle] = vrep. simxGetJointPosition(clientID, handle rigArmjoint4, vrep. simx opmode 
oneshot wait); 


[res,r5angle] = vrep. simxGetJointPosition(clientID, handle rigArmjoint5, vrep. simx opmode 
oneshot wait); 


[res,r6angle] = vrep. simxGetJointPosition(clientID, handle rigArmjoint6, vrep. simx opmode _ 
oneshot wait); 


[res,r7angle] = vrep. simxGetJointPosition(clientlID, handle rigArmjoint7, vrep. simx opmode _ 
oneshot wait); 

rl- [rl rlangle]; 

r2 = [r2 r2angle]; 

r3- [r3 r3angle]; 

r4- [r4 r4angle]; 

r5- [r5 r5angle]; 

r6- [r6 r6angle]; 

r7- [r7 r7angle]; 

end 


os 
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fid = fopen('r.txt', 'wt'); 
[m,n] = size(r); 
for i=1:1:» 
for j=1:1:n 
if j==n 
fprintf (fid, ' s g\n',r(i,j)); 
else 
fprintf (fid, '% g\t',r(i,j)); 
end 
end 
end 
fclose(fid); 


% Before closing the connection to V — REP, make sure that the last command sent out had 
time to arrive. You can guarantee this with (for example): 
vrep. simxGetPingTime(clientID); 
% Now close the connection to V- REP: 
vrep. sinxFinish(clientID); 
else 
disp('Failed connecting to remote API server'); 
end 
vrep.delete(); % call the destructor! 


disp('Program ended'); 
end 


2. 设 定 V-REP 中 机 器 人 模型 的 关节 和 角 

在 V-REP 的 场景 中 ,添加 一 个 Baxter 机 器 人 。 然 后 在 MATLAB 中 保存 下 面 的 函数 。 
先 运行 V-REP ,再 运行 下 面 的 函数 , 即 可 通过 MATLAB 给 机 器 人 设 定 每 一 个 时 刻 关 节 角 
的 数值 ,V-REP 中 的 机 器 人 将 跟随 着 关节 角 进 行 移动 。 

MATLAB 函数 如 下 : 


function BaxterTest() 
clear; 
clc; 
disp('Program started'); 
% vrep= remApi('remoteApi', 'extApi.h'); % using the header (requires a compiler) 
vrep- remApi('remoteApi'); % using the prototype file (remoteApiProto.m) 
vrep.simxFinish( —1); % just in case, close all opened connections 
clientID- vrep. simxStart('127. 0. 0.1', 19999, true, true, 5000,5); 


* read the joint angle data from 'r.txt' 
jointValue = load('left. dat'); % A matrix of 7 x 150. Each column vector recorded the 
changes of each joint Angle 
jointValue 
[m n] = size( jointValue); 
m 
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if (clientID>- 1) 
disp( 'Connected to remote API server'); 
% get handle for Baxter leftArm jointl 
[res, handle leftArmjointl] = vrep. simxGetObjectHandle(clientID, 'Baxter leftArm | 
jointl',vrep.simx opmode oneshot wait); 
k=4 
[res, handle leftArmjoint2] = vrep. simxGetObjectHandle (clientID, 'Baxter leftArm_ 
joint2',vrep.simx opmode oneshot wait); 
[res, handle leftArmjoint3] = vrep. simxGetObjectHandle (clientID, 'Baxter leftArm 
joint3',vrep.simx opmode oneshot wait); 
[res, handle leftArmjoint4] = vrep. simxGetObjectHandle (clientID, 'Baxter leftArm 
joint4',vrep.simx opmode oneshot wait); 
[res, handle leftArmjoint5] = vrep. simxGetObjectHandle (clientID, 'Baxter leftArm 
joint5',vrep.simx opmode oneshot wait); 
[res, handle leftArmjoint6] = vrep. simxGetObjectHandle(clientID, 'Baxter leftArm | 
joint6',vrep.simx opmode oneshot wait); 
[res, handle leftArmjoint7] = vrep. simxGetObjectHandle (clientID, 'Baxter leftArm 
joint7',vrep.simx opmode oneshot wait); 


% Set the position of every joint 
while(vrep. simxGetConnectionId(clientID) ~ -- 1), % while v- rep connection is 
% still active 
for i=1:m 
vrep. simxPauseCommunication(clientID, 1); 


vrep. simxSetJointTargetPosition(clientID, handle leftArmjointl, jointValue(i, 1), vrep. simx 
opmode oneshot); 
jointValue(i,1) 


vrep. simxSetJointTargetPosition(clientID, handle leftArmjoint2, jointValue(i, 2), vrep. simx 
opmode oneshot); 


vrep. simxSetJointTargetPosition(clientID, handle leftArmjoint3, jointValue(i, 3), vrep. simx 
opmode oneshot); 


vrep. simxSetJointTargetPosition(clientID, handle leftArmjoint4, jointValue(i, 3), vrep. simx 
opmode oneshot); 


vrep. simxSetJointTargetPosition(clientID, handle leftArmjoint5, jointValue(i, 5), vrep. simx 
opnmode oneshot); 


vrep. simxSetJointTargetPosition(clientID, handle leftArmjoint6, jointValue(i, 6), vrep. simx 
opnmode oneshot); 


vrep. sinxSetJointTargetPosition(clientID, handle leftArmjoint?7, jointValue(i, 7), vrep. simx 
opnmode oneshot); 
vrep. simxPauseCommunication(clientID, 0); 
pause(0.1); 
end 
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vrep. simxGetConnectionId(clientID) = 1; 
end 


* Before closing the connection to V- REP, make sure that the last command sent out had 
% time to arrive. You can guarantee this with (for example): 
vrep. simxGetPingTime(clientID); 
* Now close the connection to V- REP: 
vrep. simxFinish(clientID); 
else 
disp('Failed connecting to remote API server'); 
end 
vrep.delete(); % call the destructor! 


disp('Program ended'); 
end 


3.11 V-REP 在 人 机 交互 中 的 应 用 (一 ) 


3.11.1 触觉 学 与 Touch X 


在 20 世纪 初 ,在 研究 基于 人 类 触摸 的 感知 和 操作 的 课题 上 ,触觉 学 (Haptics) 第 一 次 被 
心理 物理 学 家 提出 。 之 后 ,在 20 世纪 70 年 代 , 在 另外 一 个 研究 领域 一 一 机 器 人 技术 中 , 尤 
其 在 自主 机 器 人 的 研究 中 ,开始 关注 通过 力 /触觉 来 进行 感知 和 操作 。 在 21 世纪 初 ,由 于 多 
种 新 兴 技 术 的 融合 与 发 展 ,计算 机 触觉 技术 得 到 了 快速 的 发 展 。 这 种 新 的 技术 使 得 能 够 通 
过 触觉 接口 ,给 人 类 的 手 施 加 一 定 的 力 , 从 而 传递 信息 。 而 这 种 力 的 传递 ,依靠 着 机 械 的 物 
理 接触 。 与 人 类 的 视觉 .听觉 .嗅觉 等 相 比 , 力 /触觉 的 能 量 和 信息 的 流向 是 双向 的 ,因此 能 
够 达到 良好 的 交互 体验 。 

力 /触觉 设备 包括 了 力 再 现 设备 和 触觉 再 现 设备 。 力 再 现 设备 能 够 将 远 端 非 现场 或 虚 
拟 环境 中 的 物体 力 特 性 传递 给 使 用 者 ,包括 了 物体 的 轻重 .柔软 程度 等 。 现 在 已 有 的 力 再 现 
设备 有 外 骨骼 和 固定 设备 、 数 据 手套 和 穿戴 设备 、 点 交互 设备 和 专用 设备 、 基 于 智能 材料 的 
力 /触觉 装 置 。 触 觉 再 现 设备 能 够 将 它们 的 触觉 信息 传递 给 使 用 者 ,包括 了 物体 的 粗糙 程度 
和 形状 等 。 现 在 已 有 的 触觉 再 现 设备 主要 按照 驱动 方式 进行 分 类 ,目前 有 电磁 驱动 .电机 了 驱 
动 .气体 驱动 . 压 电 陶瓷 驱动 .记忆 合金 的 SMA 线 驱 动 等 多 种 驱动 方式 。 

Geomagic Touch X( 原 名 为 Sensable Phantom Desktop) 是 美国 Geomagic 公司 的 力 触 
觉 设备 类 的 产品 。Touch X 具有 3 个 自由 的 维度 (上 下 翻动 .左右 晃动 和 侧 向 移动 ) ,6 个 角 
度 的 自由 操作 位 置 传 感 ,能 够 通过 力 反馈 提供 真实 的 三 维 输入 。Touch X 的 使 用 是 固定 在 
桌面 上 的 ,用 户 通 过 Touch X 的 终端 效应 器 进行 操作 ,就 好 像 使 用 其 他 工具 与 虚拟 环境 或 
远程 环境 进行 交互 。 

Touch X 的 耐久 性 负担 能 力 和 准确 性 :尤其 是 简洁 紧密 性 .可 移植 性 ,使 得 它 可 用 于 与 
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数字 世界 进行 真实 的 互动 ,推动 了 它 在 商业 、 医 学 和 科学 研究 上 的 应 用 。 目 前 ,Touch X 已 
经 在 机 器 人 控制 、 虚 拟 装 配 \ 三 维 建 模 、 娱 乐 与 虚拟 现实 等 领域 上 广泛 应 用 。 


3.11.2 Touch X 的 相关 软件 在 人 机 交互 中 的 作用 


与 Touch X 相关 的 软件 包括 了 Geomagic Touch Device Driver 和 OpenHaptics 
Developer Edition。 前 者 是 Touch X 的 驱动 程序 ,包括 了 用 于 连接 Touch X 和 计算 机 的 
Geomagic Touch Setup, 以 及 用 于 校准 诊断 Touch X 的 Geomagic Touch Diagnostic。 后 者 是 
Touch X 的 开源 软件 开发 包 , 在 与 Touch X 与 CHAI3D 的 连接 中 ,起 到 了 中 间 桥 梁 的 作用 。 

1. Geomagic Touch Setup 

当 Touch X 直接 通过 网 线 与 计算 机 连接 时 ,Touch X 可 在 计算 机 中 正常 使 用 ,但 在 
Geomagic Touch Setup 的 界面 出 现 “No device available” 的 提示 。 此 时 ,应 该 使 用 通过 USB 
网 线 转 接 器 连接 ,如 图 3-129 所 示 ,正确 的 接 法 是 : 计算 机 -USB 线 缆 -USB 网 线 转 接 器 -网 
线 -Touch X。 在 这 其 中 ,USB 网 线 转 接 器 可 起 到 转换 与 保护 作用 。 当 Touch X 与 计算 机 连 
接 正常 时 ,在 Geomagic Touch Setup 的 界面 中 , 单 击 Pairing ,并 同时 按 下 Touch X 背后 的 
连接 按钮 时 ,将 会 出 现 表示 连接 成 功 的 命令 ,这 才 可 进入 下 一 步 的 使 用 。 


USB 线 绕 


USB- 网 线 
转 接 器 


LUI 


图 3-129. Touch X 与 计算 机 的 连接 


2. Geomagic Touch Diagnostic 

Geomagic Touch Diagnostic 的 主要 功能 是 对 Touch X 的 校准 和 诊断 。 在 Geomagic 
Touch Diagnostic 的 界面 中 ,提供 了 校准 . 读 取 编 码 器 测试 ,周期 放大 器 测试 . 力 测 试 、 盒 测 
试 和 伺服 回路 测试 功能 。 在 其 中 的 校准 界面 中 ,可 测试 Touch X 向 上 /向 下 、 向 左 / 向 右 、 向 
外 /向 里 的 功能 。 当 使 用 者 在 所 对 应 的 方向 进行 测试 时 ,如 若 Touch X 的 功能 正常 ,测试 界 
面相 应 的 图 标 会 由 红色 变 绿色 。 在 编码 器 测试 界面 ,使 用 者 在 移动 Touch X 时 ,界面 将 实 
时 地 显示 关于 Touch X 的 位 置 等 数据 。 在 周期 放大 器 测试 界面 ,将 会 测试 Touch X 的 放大 
功能 是 否 正常 。 在 力 测 试 界面 中 ,使 用 者 在 使 Touch X 向 上 下 左右、 前 后 移动 时 ,界面 会 
实时 显示 出 各 个 方向 的 力 。 在 盒 测试 界面 中 ,提供 了 一 个 含有 一 颗 小 球 的 正方 形 形 状 框 ,使 
用 者 在 移动 Touch X 中 ,边框 中 的 小 球 会 跟随 着 地 移动 。 当 小 球 触 碰 到 边框 时 ,界面 会 实 
时 显示 出 小 球 对 边框 的 作用 力 , 另 外 使 用 者 的 手 也 会 感受 到 一 定 的 力 。 在 伺服 回路 测试 界 
面 中 ,使 用 者 在 移动 Touch X 时 ,界面 会 记录 Touch X 的 使 用 频率 ,数值 一 般 都 在 1000Hz 
附近 。 总 而 言 之 .在 Geomagic Touch Diagnostic 中 ,主要 提供 了 对 Touch X 的 测试 ,判断 它 
是 否 能 被 正常 地 使 用 。 这 一 步骤 是 下 一 步 对 Touch X 进行 使 用 的 重要 前 提 。 
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3. OpenHaptics Developer Edition 

OpenHaptic 工具 套件 在 高 度 级 别 上 .为 已 经 使 用 OpenGL. 技术 的 应 用 程序 启用 了 
Sensable Phantom 触觉 设备 支持 、 真 实 3D 导航 、 材 料 属性 和 多 边 形 对 象 支持 。 在 较 低级 别 
上 ,为 开发 人 员 提 供 了 对 传感器 读数 \、 设 备 控制 、 力 泻 染 的 直接 控制 以 及 其 他 功能 的 完全 访 
问 权 。 其 中 包括 了 QuickHaptics 微型 应 用 程序 , 它 能 够 使 开发 者 利用 示例 代码 ,对 触觉 应 
用 程序 进行 快速 开发 。 

QuickHaptics 微型 应 用 程序 有 两 个 功能 ,一 个 让 使 用 者 能 够 更 快 更 方便 地 编写 新 的 力 
触觉 应 用 程序 , 另 一 个 是 给 外 部 的 应 用 程序 添加 力 触觉 功能 ,使 得 其 他 的 程序 能 够 借助 
QuickHaptics 与 触觉 设备 连接 。 在 与 Touch X 5j V-REP 的 通信 中 ,QuickHaptics 主要 实 
现 了 第 二 种 功能 ,成 为 二 者 Touch X 与 CHAI3D 之 间 的 连接 桥梁 。 

Quickhaptics 包含 了 图 形 绘制 和 触觉 功能 , 它 为 典型 的 触觉 应 用 程序 封装 了 人 逻辑 步 又 ， 
使 用 户 可 以 快速 地 进行 程序 设计 和 程序 部 署 。 这 些 已 经 封装 好 的 多 辑 步 又 包括 ; 

CD 包括 了 流行 的 动画 包 中 的 几何 文件 ; 

@ 对 图 形 环境 进行 了 初始 化 ; 

© 对 触觉 环境 进行 了 初始 化 ; 

@ 搭建 了 虚拟 的 场景 ; 

C) 将 触觉 参数 映射 到 场景 对 象 中 ; 

© 为 触觉 设备 与 场景 的 交互 做 出 反应 。 

因此 ,经 过 了 这 些 逻 辑 步骤 的 封装 后 ,用 户 只 需要 通过 使 用 默认 值 的 参数 ,进行 相关 代 
码 的 编写 ,就 可 以 创建 一 个 可 行 的 场景 ,而 不 需要 指定 特殊 的 位 置 .设备 空间 参数 或 各 种 形 
状 属性 的 值 。 


3.11.3 CHAI3D 在 人 机 交互 中 的 作用 


1. CHAI3D 介绍 

CHAI3D 是 2003 年 由 斯 坦 福 大 学 机 器 人 与 人 工 智能 实验 室 开发 的 一 款 跨 平台 的 C++ 
仿真 框架 。 目 前 ,全 世界 超过 100 个 工业 企业 和 科研 机 构 已 经 将 CHAI3D 用 于 汽车 ,航天 航 
空 ,医疗 技术 和 机 器 人 等 多 个 领域 。 

CHAI3D 是 一 组 开源 的 C++ 函数 库 . 它 可 用 于 计算 机 触觉 技术 、 可 视 化 和 人 机 交互 式 
的 实时 仿真 。 作 为 一 款 开源 的 仿真 框架 ,CHAI3D 支持 多 种 商业 化 的 三 自由 、 六 自由 度 , 七 
自由 度 的 力 触觉 设备 。CHAI3D 适合 用 于 教育 研究 和 专业 的 应 用 ,给 它们 提供 一 个 可 扩 
展 的 开发 平台 。CHAI 的 模块 化 功能 允许 创建 高 性 能 的 本 机 触觉 应 用 程序 ,让 使 用 者 可 以 
达到 良好 的 触觉 和 视觉 用 户 体验 。 

2. CHAI3D 的 安装 

从 CHAI3D 的 主页 (http://www. chai3d. org/download/releases) 下 载 相 对 应 
Windows 平 台 的 CHAI3D 资源 包 后 ,需要 经 过 以 下 步骤 才能 进行 安装 。 

CD 确保 计算 机 中 已 安装 Visual Studio. 并 能 够 正常 运行 。 目 前 只 支持 2010、2012、 
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2013,2015 的 Visual Studio 版 本 ; 

© 利用 Visual Studio 打开 相对 应 版 本 的 sln 工程 文件 ,如 使 用 Visual Studio 2015 , 则 
需 打开 文件 名 为 CHAI3D-VS2015. sln 的 工程 文件 ; 
C 选择 构建 配置 和 目标 架构 : 解决 方案 配置 选择 为 Release, 解 决 方案 平台 选择 为 X64; 
QD 在 Visual Studio 的 生成 窗口 中 选择 生成 解决 方案 ,系统 将 自 ie 生成 相对 应 的 
CHAI3D 函数 库 和 测试 例子 。 

3. CHAI3D 的 测试 

ia 以 上 对 Visual Studio 工程 文件 的 编译 后 ,将 会 生成 相应 的 解决 方案 ,以 及 它 包含 
的 项 目 。 需 要 通过 对 例子 的 测试 使 用 ,判断 CHAI3D 是 否 安 装 正常 与 力 触觉 设备 是 否 支持 
CHAI3D 函数 库 。 

选择 CHAI3D 提供 的 一 个 例子 ,在 Visual Studio 的 调试 窗口 中 选择 调试 功能 ， 
bebe 自动 弹出 以 下 的 窗口 。 如 若 设备 未 连接 ,或 者 并 未 安装 相应 的 OpenHaptics f& 
如 若 连接 正常 ,将 会 显示 如 下 的 效果 

如 图 3-130 所 示 , 窗 口 的 上 方 显示 一 颗 小 球 ,并 在 它 本 身 建 立 起 一 个 XX 轴 、Y 轴 、Z 轴 构 
成 的 直角 坐标 系 。 当 用 户 用 手 操作 Touch X 时 ,小 球 会 相应 地 运动 。 当 小 球 离开 原始 位 置 
的 距离 越 远 ,用 户 的 手 将 会 感受 到 越 大 的 反馈 力 。 同 时 ,窗口 下 方 记录 了 小 球 的 运动 状态 ， 
其 中 不 同 颜色 的 线 分 别 代表 了 小 球 在 各 个 方向 的 位 移 ; 右边 的 格子 实时 地 显示 小 球 移动 的 
速度 ; 右边 的 圆圈 显示 了 小 球 在 各 个 方向 受到 的 力 


,窗口 将 显示 “No devices 


PHANTOM Desktop 
0.043, 0.002, 0.078 


图 3-130 CHAI3D 的 测试 例子 
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3.11.4 V-REP 模块 


1. 力 触觉 设备 与 V-REP 的 交互 原理 

在 CHAI3D 的 函数 库 中 ,存在 着 4 个 模块 : GEL 模块 .OCULUS 模块 .ODE 模块 和 
V-REP 模块 。 这 些 模块 可 以 被 单独 或 混合 地 被 使 用 。 

如 图 3-131 所 示 , 在 V-REP 模块 中 ,通过 — CHAI3D 
CHAI3D 函数 库 连 接 V-REP 和 和 触觉 设备 的 
原理 是 : CHAI3D 中 的 V-REP 模块 为 V-REP OpenHaptics 
软件 创建 了 一 个 可 分 享 的 函数 库 。CHAI3D Sa 
插件 通过 使 用 chai3d:: cWorld 这 个 类 创建 
一 个 无 形 的 CHAI3D 场景 ,并 且 同 时 运行 
V-REP 的 仿真 。 fm 

这 个 CHAI3D 场景 可 以 被 V-REP 的 
LUA 脚本 所 使 用 的 LUA 函数 控制 ,从 而 允 
许 使 用 完成 以 下 任务 : 

(1) 连接 V-REP 和 力 触觉 设备 。 

(2) 将 V-REP 中 的 物体 复制 到 CHAI3D 
中 的 场景 中 ,因此 这 些 物 体能 够 被 力 触 觉 设 
备 所 感受 。 

(3) 当 V-REP 进行 仿真 时 ,V-REP 中 的 
物体 会 移动 。CHAI3D 场景 中 所 对 应 的 物体 将 进行 同样 的 移动 , 以 保持 相应 的 位 置 和 
姿态 。 

(4) 限制 力 触 觉 设 备 的 行为 。 例 如 对 于 一 个 二 维 的 力 反馈 操作 杆 , 仿 真 时 将 会 把 它 的 
行为 限制 在 一 个 平面 上 。 

(5) 在 仿真 中 改变 力 触觉 设备 的 约束 特性 ,用 来 反映 变化 。 例 如 ,在 V-REP 中 ,设置 一 
定 的 障碍 物 , 当 仿真 模型 接近 障碍 时 , 力 触 觉 设备 会 反馈 一 定 的 力 , 用 来 表示 模型 与 障碍 的 
距离 。 

2. V-REP 模块 的 安装 

与 CHAI3D 资源 包 的 安装 步骤 相 类 似 ,V-REP 模块 也 需要 以 下 的 步骤 才能 够 正确 
安装 : 

利用 Visual Studio 打开 相对 应 版 本 的 sln 工程 文件 ,这 里 使 用 Visual Studio 2015 
打开 命名 为 CHAI3D-V-REP-VS2015. sln 的 工程 文件 。 

© 选择 构建 配置 和 目标 架构 : 解决 方案 配置 选择 为 Release, 解 决 方案 平台 选择 为 X86 
(由 于 V-REP 为 32 位 ,这 里 需 选 择 X86)。 

®© 在 Visual Studio 的 生成 窗口 中 选择 生成 解决 方案 , 系统 将 自动 创建 库 
v_repExtCHAI3D. lib 和 对 象 v_repExtCHAI3D. exp, 并 且 生 成 文件 v_repExtCHAI3D. dll, 


图 3-131 Touch X,CHAI3D 与 
V-REP 之 间 的 交互 
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CD 将 生成 的 文件 复制 到 V-REP 安装 文件 夹 中 (如 路 径 为 : C:\Program Files (x86)\ 
V-REP3\V-REP_PRO_EDU) ,这 里 包含 的 文件 有 v_repExtCHAI3D. dll .hdPhantom32. dll 
等 多 个 文件 。 

3. V-REP 模块 的 测试 

在 CHAI3D 中 的 V-REP 模块 安装 后 ,为 进一步 实现 Touch X 与 V-REP 进行 交互 ,使 
用 CHAI3D 提供 的 简单 例子 进行 测试 。 

当 打 开 相 应 V-REP 的 . ttt 文件 时 ,控制 窗口 将 显示 :“Add-on script 'vrepAddOnScript- 
addOnScriptDemo. lua' was loaded”, 这 表示 CHAI3D 的 插件 已 成 功 安装 。 

这 里 对 原 有 的 例子 场景 进行 了 修改 和 完善 ,并 把 原来 场景 中 分 布 的 圆柱 体 障 碍 改造 成 
两 条 路 障 ,给 导航 机 器 人 修筑 了 一 条 曲 形 弯 道 。 修 改 后 的 场景 如 图 3-132 所 示 , 它 提供 了 一 
个 通过 力 触 觉 设 备 控制 的 导航 机 器 人 以 及 周围 环境 。 通 过 力 触觉 设备 ,可 以 控制 机 器 人 的 
速度 和 方向 。 当 机 器 人 接近 障碍 时 , 力 触觉 设备 会 将 力 传 给 使 用 者 ,让 使 用 者 感受 到 路 障 位 
于 机 器 人 的 哪个 方向 。 


图 3-132. V-REP 5j Touch X 的 交互 


通过 测试 ,可 以 得 出 这 样 的 实验 结果 : 
(OD. 当 V-REP 仿真 开始 时 ,Touch X 会 收 到 反馈 ,操作 杆 将 回 到 起 始 的 位 置 。 
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(2) 使 用 者 通过 Touch X 的 操作 杆 , 可 以 使 机 器 人 移动 ,控制 它 的 速度 和 方向 。 

(3) 在 机 器 人 前 进 的 过 程 中 ,通过 V-REP 提供 的 近 距 离 传 感 器 ,会 测量 机 器 人 与 路 障 
之 间 的 距离 ,并 把 距离 的 大 小 转化 为 力 的 强 弱 ,通过 Touch X 传 到 使 用 者 的 手 上 。 

(4) 当 机 器 人 距离 路 障 越 近 时 ,使 用 者 感受 到 的 力 越 大 。 当 距离 小 到 一 定 的 程度 时 ， 
Touch X 将 会 出 现 不 受 控 的 情况 , 弹 到 一 定 的 位 置 ,而 V-REP 中 的 导航 机 器 人 也 会 随 着 移 
动 到 相应 的 位 置 。 

Touch X 与 V-REP 连接 失败 有 两 种 情况 ,第 一 种 是 CHAI3D 相关 的 插件 没有 安装 成 
功 ,第 二 种 是 力 触觉 设备 没 与 计算 机 连接 成 功 。 在 第 一 种 情况 中 ,V-REP 的 仿真 结果 是 主 
界面 下 方 出 现 相关 的 代码 错误 提示 : Lua API call error: [string "SCRIPT hapticDevice" ]: 
302; Initialization failed. (simExtCHAI3D_start)。 在 第 二 种 情况 中 , V-REP 的 仿真 结果 
是 在 主 界面 中 ,弹出 错误 提示 窗口 ,显示 : Device failed to initialize。 


3.11.5 Touch X 控制 V-REP 中 KUKA 机 器 人 的 实现 


1. CHAI3D 中 LUA 函数 的 使 用 

1) V-REP 与 Touch X 通信 的 函数 

通过 Lua 语句 可 以 实现 V-REP 和 Touch X 的 通信 连接 ,主要 由 以 下 LUA 函数 : 
simExtCHAI3D_start()。 当 运行 该 函数 时 ,返回 值 为 1 则 表示 通信 成 功 ,返回 值 为 一 1 则 
表示 通信 失败 。 

2) CHAI3D 场景 中 的 两 种 对 象 

在 CHAI3D 的 插件 中 ,有 两 种 不 同 的 对 象 ,用 来 产生 所 有 CHAI3D 场景 中 的 触觉 行为 。 
这 两 种 对 象 分 别 是 形状 对 象 和 约束 对 象 。 形 状 对 象 可 以 用 在 CHAI3D rp , 泻 染 V-REP 中 所 
有 存在 对 象 ; 而 约束 对 象 可 以 用 于 控制 力 触觉 设备 的 行为 ,以 用 来 帮助 使 用 者 完成 一 些 特 
别 的 任务 。 

3) 形状 对 象 

当 需 要 在 CHAI3D 添加 存在 于 V-REP 中 的 所 有 对 象 时 ,需要 用 到 的 Lua 函数 为 : 
simExtCHAI3D_addShape() 。 当 使 用 该 函数 后 ,对象 将 被 添加 到 CHAI3D 的 场景 中 ,此 时 
对 象 可 以 被 连接 到 CHAI3D 场景 中 的 力 触觉 设备 所 感知 ,并 且 能 够 反馈 相应 的 力 。 

当 V-REP 中 的 对 象 在 进行 仿真 时 ,发 生 了 一 定 的 位 置 或 姿态 变化 时 ,CHAI3D 相对 应 
的 对 象 可 以 通过 以 下 Lua 的 函数 ,实现 同时 变化 : simExtCHAI3D_updateShape() 。 

4) 约束 对 象 

在 CHAI3D 的 约 东 对象 中 ,有 约束 点 、 约 东 线 段 和 约 东 平面 三 种 类 型 ,它们 被 用 于 约束 
力 触觉 设备 的 末端 执行 器 。 

将 力 触觉 设备 的 末端 执行 器 约束 在 一 个 单独 的 点 ,使 用 的 Lua 函数 为 : 


SimExtCHAI3D addConstraintPoint() 


将 力 触觉 设备 的 末端 执行 器 约束 在 一 条 线段 .使 用 的 Lua 函数 为 : 
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simExtCHAI3D addConstraintSegment() 


将 力 触觉 设备 的 末端 执行 器 约束 在 一 个 平面 ,使 用 的 Lua 函数 为 


SimExtCHAI3D addConstraintPlane() 


在 使 用 这 三 种 约束 对 象 时 ,都 遵守 着 以 下 三 个 公式 : 
F, = K, X Su X D 
Jp FEAREN ,表示 维持 约束 对 象 与 约束 目标 之 间距 离 所 使 用 的 力 , 它 会 随 着 二 者 之 间 
距离 的 变化 而 变化 ; KK, 为 刚度 比例 因子 ; Sw 为 设备 所 能 支持 的 最 大 刚度 ; D 为 约束 对 象 与 
约束 目标 的 距离 。 
F, = K, X Du XV 
Hp. F AEJ ,表示 设备 在 遇 到 约束 对 象 时 受到 的 力 ,使 得 设备 会 瞬间 移动 ; K, Akh E 
比例 因子 ; Dw 为 设备 所 能 支持 的 最 大 阻尼 ; V. 为 设备 移动 的 速度 。 
F, = min(Fw,F) 十 下 。 
其 中 ,FF, 为 设备 受到 总 体 的 力 ; Fw 为 最 大 的 受 力 限制 ,用 于 限制 由 于 刚度 产生 的 力 , 达 到 保 
护 设备 的 效果 。 
上 面 三 个 公式 中 存在 的 参数 可 通过 以 下 Lua 函数 进行 调节 : 


SimExtCHAI3D updateConstraint() 


2. 虚拟 交互 环境 的 搭建 与 测试 

首先 ,从 V-REP 中 的 机 器 人 模型 浏览 器 中 导入 型 号 为 LBR iwaa 7 R800 的 KUKA 机 
器 人 ,这 是 一 款 小 型 的 工业 机 器 人 ,其 重量 为 23. 9kg, 具 有 7 个 轴 , 最 大 负荷 为 7kg, 可 进行 
人 机 协作 ,用 于 机 械 加 工 、 装 货 等 领域 。 

在 V-REP 中 ,KUKA R800 机 器 人 具有 6 个 关节 ,因此 使 得 机 器 人 具有 6 个 自由 度 。 
分 别 单 击 每 个 关节 的 对 话 框 ,把 它们 的 关节 模式 设置 为 逆 运 动 学 模式 (joint is in inverse 
kinematics mode) ,并 改变 默认 的 最 大 力矩 和 最 高 速度 限制 ,以 达到 更 好 的 控制 效果 (在 本 
实验 中 ,以 上 两 个 参数 都 各 增加 一 倍 ) 。 

接 下 来 ,需要 定义 一 个 道 运动 学 的 任务 。 在 V-REP 中 , 逆 运 动 学 至 少 需要 规范 以 下 两 
个 任务 : 

(D 一 个 运动 链 描述 了 一 个 虚拟 的 末端 和 一 个 基本 的 对 象 ; 

(2) 一 个 虚拟 的 目标 使 得 应 虚拟 的 末端 在 运动 上 受到 限制 。 

因此 ,在 接 下 来 的 步 又 中 ,添加 了 一 个 虚拟 的 目标 对 象 和 一 个 虚拟 的 末端 对 象 ,并 把 它 
们 的 位 置 与 姿态 进行 了 适当 的 设置 (其 中 虚拟 的 末端 对 象 的 位 置 与 KUKA 机 器 人 的 末端 
一 致 )。 如 图 3-133 Bros ,然后 ,打开 逆 运 动 学 对 话 框 ,添加 一 组 新 的 IK 组 合 , 在 TK 元 素 对 
话 框 中 添加 虚拟 的 末端 。 以 上 的 设置 ,使 得 虚拟 的 末端 将 跟随 虚拟 的 目标 进行 位 置 和 姿态 
上 的 变化 。 
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& kJ resp 
4. LBR. iiwa, 7. R800 link3 
$9. 300. joint. 

BT- b LB e. 7 800 link4 resp 

4. LER. ie. 7. P800, lnk4 

Br É LBF, ive 7. 800, join 
Br @ LER jio, 7. 00, link, resp 

44 LBR_wa_7_R800_ink5 

8 LBRLiwa_7 F800 joins 

Ey- @ LER, ja 7. R00, linkf, resr 

4D. LER. iiwe. 7. R00, link 

rA 6 


B 


© redundentrobot tip 
LBR., siwa 7. R00 joi 


ER. jiwa 7. R00 j 
LBR ive. 7 R800 hnkB resp 
日 一 È Force sensor 

4) LBR. we, 7. R800 JinkB. 


图 3-133 V-REP 场景 KUKA 机 器 人 工作 环境 的 搭建 


最 后 ,在 V-REP 的 脚本 对 话 框 中 添加 一 个 非 线程 的 子 脚本 ,并 通过 设置 将 子 脚本 添加 
至 名 为 “LBR_iwaa_7_R800” 的 对 象 中 。 在 子 脚 本 的 代码 编写 中 ,利用 CHAI3D 插件 提供 的 
Lua 函数 进行 编程 ,以 进行 Touch X 5j V-REP 的 连接 。 

子 脚 本 的 主要 代码 流程 如 下 : 

CD 判断 CHAI3D 插件 是 否 安装 ,如 果 插 件 未 安装 界面 将 显示 错误 ; 

© 如 果 CHAI3D 插件 安装 正确 .将 通过 simExtCHAI3D_reset() 函 数 对 插件 进行 重新 
设置 ; 并 通过 simExtCHAI3D_start() 函 数 对 设备 的 工具 半径 和 工作 范围 进行 设 定 ， 

@ 检测 设备 是 否 连接 正常 ,如 果 设 备 连接 不 正常 界面 将 提示 错误 ， 

@ 对 KUKA 机 器 人 相关 的 对 象 进行 处 理 , 包 括 各 个 关节 ,虚拟 的 末端 和 虚拟 的 目标 ; 

(5) 通过 simExtCHAI3D_readPosition( ) 函数 读 取 Touch X 控制 的 光标 所 在 位 置 ; 

© 通过 simSetObjectPosition O 函数 将 新 的 位 置 设置 为 虚拟 目标 的 位 置 ， 

CD 对 第 5、 第 6 步骤 进行 循环 ,使 得 V-REP 通过 CHAI3D 插件 不 断 读 取 Touch X 的 新 
位 置 , 并 把 新 的 位 置 传递 给 KUKA 机 器 人 末端 。 

相关 的 Lua 代码 为 : 


addSphere = function(rad, pos) 

if CHAI3DPluginInitialized then 
local sphereHandle = sinCreatePureShape(1, 4 + 16, (rad * 2, rad * 2, rad * 2),1) 
simSetObjectParent(sphereHandle, modelHandle, true) 
sinSetObjectPosition(sphereHandle, modelHandle, pos) 
return addMesh( sphereHandle, true), sphereHandle 

end 

Toton - 1, — E 
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end 


addMesh = function(shapeHandle, whenRemovedEraseMesh) 
if CHAI3DPluginInitialized then 
if not objCont then 
objCont = {} 
end 
objCont[ & objCont + 1] = shapeHandle 
local vertices, indices - simGetShapeMesh( shapeHandle) 
local pos = simGetObjectPosition(shapeHandle, nodelHandle) 
local orient = simGetObjectOrientation( shapeHandle, modelHandle) 
local hapticObjId = simExtCHAI3D addShape(vertices, indices, pos, orient, 1000) 
objCont[ # objCont + 1] = hapticObjld 
objCont[ # objCont + 1] = whenRemovedEraseMesh 
return hapticObjId 
end 
return - 1 
end 
setObjectPose = function(objectHandle, pos, orient, interpolTimeInMs) 
if CHAI3DPluginInitialized then 
if objCont then 
for i= 1,( # objCont) /3, 1 do 
local shapeHandle = objCont[3 x (i- 1) *1] 
local fdobj = objCont[3 (1-1) *2] 
if fdobj == objectHandle then 
simSetObjectPosition(shapeHandle, modelHandle, pos) 
simSetObjectOrientation(shapeHandle, nodelHandle, orient) 
SimExtCHAI3D updateShape(fdObj, pos, orient, 1000) 
return 


if (sim call type-- sim childscriptcall initialization) then 
modelHandle = simGetObjectAssociatedWithScript(sim handle self) 
modelBase = simGetObjectAssociatedWithScript(sim handle self) 


—- check if the haptic plugin is loaded 
moduleName - 0 
moduleVersion - 0 
index - 0 
pluginNotFound - true 
while (moduleName) do 


end 
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moduleName, moduleVersion = simGetModuleName( index) 
if (moduleName == 'CHAI3D') then 
pluginNotFound = false 
end 
index = index t1 


if (pluginNotFound) then 


simDisplayDialog ( ' Error ', 'CHAI3D plugin was not found, or was not correctly 


initialized (v repExtCHAI3D.dll).', sim dlgstyle ok, false, nil, (0.8, 0, 0, 0, 0, 0}, (0.5, 
0; 0-1; 


1, 1}) 


else 


maxVel = 


end 


local device = 0 

local toolRadius = 0.3 

workspaceRadius = 0.2 

if (simExtCHAI3D start(device, toolRadius, workspaceRadius) ==- 1) then 
simDisplayDialog('Error', 'Device failed to initialize.', sim dlgstyle ok, false, 


nil, 10:8, 00, 0-0, 0p 10:50, 0 2 51721) 


else 
CHAI3DPluginInitialized = true 
forcesensor = simGetObjectHandle( 'Force_sensor') 
FR simReadForceSensor(forcesensor, 1,0) 


dummyTarget = simGetObjectHandle( 'redundantrobot target') 
dummyTip = simGetObjectHandle( 'redundantrobot tip') 


-— Create a haptic sphere: 

local sphereRadius = 0.03 

local spherePosition- (0, — 0.4,0.04) 

sphereld, sphereHandle = addSphere( sphereRadius, spherePosition) 


0.2 
end 


if (sim call type-- sim childscriptcall cleanup) then 


if (CHAI3DPluginInitialized) then 


end 


SimExtCHAI3D reset() 


if (sim call type-- sim childscriptcall sensing) then 


if CHAI3DPluginInitialized then 
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—— Read the position of the cursor: 
local p = simExtCHAI3D readPosition(0) 


—- Reflect the position of the cursor in V — REP: 
—- simSetObjectPosition(toolSphereHandle, modelHandle, p) 


—— Read the interaction force: 
local f = simExtCHAI3D readForce(0) 


—— Read the buttons of the device: 
local currentButtonStates = simExtCHAI3D readButtons(0) 


local s = 0. 01/simGetSimulationTimeStep() 
local dummyTargetPos = simGetObjectPosition(dummyTarget, - 1) 
local p= simExtCHAI3D readPosition(0) 

print(p[1]) 

print(p[2]) 

print(p[3]) 
local newPos = (p[1] * 3, p[2] * 3, p[3] * 3) 
simSetObjectPosition(dummyTarget, — 1, newPos) 


end 
end 


完成 虚拟 环境 的 搭建 和 相关 代码 的 编写 后 ,并 检测 相关 设备 是 否 连接 正 在 V-REP 
界面 中 选择 仿真 ,并 通过 操作 Touch X, 其 实验 效果 如 图 3-134 所 示 。 连 接 成 功 后 ,利用 
Touch X 进行 控制 时 ,V-REP 中 的 KUKA 机 器 人 将 会 跟随 目标 位 置 实 时 地 进行 运动 ,到 达 
新 的 位 置 。 在 之 前 代码 中 有 设置 一 个 不 可 见 的 工作 区 间 , 当 机 器 人 未 端 触 碰 到 该 区 间 边 缘 
时 ,用户 将 不 能 操作 Touch X 继续 向 该 方向 移动 ,能 感受 到 一 定 的 阻力 。 此 外 ,通过 添加 力 
传 感 E XLY.Z 方向 的 受 力 


图 3-134 Touch X 控制 V-REP 中 KUKA 机 器 人 的 实验 效果 
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总 体 的 实验 效果 是 ,KUKA 机 器 人 能 够 实时 跟随 Touch X 移动 ,达到 向 上 /下 ,向 左 / 
右 , 向 前 /后 的 跟随 功能 ,并 且 Touch X 能 够 感受 到 一 定 的 力 反馈 。 从 实验 结果 得 知 , Touch 
X 能 够 通过 本 身 相关 的 驱动 软件 和 开发 软件 ,与 CHAI3D 进行 连接 ; 然后 再 通过 CHAI3D 
插件 提供 的 相关 Lua 函数 与 V-REP 进行 连接 ,从 而 达到 控制 V-REP 中 导航 机 器 人 或 工业 
机 器 人 的 功能 ,并 能 够 通过 相关 的 设置 将 虚拟 环境 中 模型 的 受 力 反馈 到 使 用 Touch X 的 用 
户 手中 ,实现 了 人 机 交互 功能 。 


3.12 V-REP 在 人 机 交互 中 的 应 用 (二 ) 


3.12.1 体感 技术 与 Kinect 


体感 技术 是 人 机 交互 中 一 种 重要 的 交互 方式 ,因为 它 主 要 通过 人 身体 的 动作 与 计算 机 
中 的 虚拟 环境 进行 交互 ,这 种 交互 方式 自然 ,友好 ,具有 实时 性 ,目前 主要 用 于 娱乐 .游戏 和 
教育 中 。 

目前 体感 技术 以 下 几 种 方式 : 惯性 感 测 .光学 感 测 、 惯 性 及 光学 联合 感 测 技术 .肌肉 电 
信号 传 感 技术 和 WiFi 信号 传 感 技术 ,其 中 光学 感 测 有 基于 光学 反射 片 或 发 光 器 的 体感 技 
术 和 视线 跟踪 技术 和 基于 光 编 码 的 人 体 成 像 技 术 这 三 种 光学 技术 。 

随 着 人 们 对 体感 技术 相关 的 图 像 处 理 . 机 器 视觉 ,传感器 技术 等 多 个 领域 的 深入 研究 ， 
体感 交互 的 技术 也 将 得 到 更 多 的 发 展 ,因此 将 能 够 应 用 到 更 多 领域 中 ,如 军事 、 勘 察 等 复杂 
的 任务 。 

Kinect 是 微软 公司 于 2010 年 为 其 游戏 主机 XBOX 360 而 研制 的 一 个 外 部 设备 产品 , 主 
要 用 于 人 体 姿态 的 输入 设备 。Kinect 具有 3 个 摄像 头 ,左边 为 红外 线 发 射 器 ,中 间 为 RGB 
彩色 摄像 机 ,右边 红外 线 CMOS 摄像 机 ,其 中 左边 和 右边 的 摄像 头 构成 了 深度 传感器 ,用 来 
捕 提 3D 的 影像 。 此 外 ,Kinect 具有 数组 式 麦 克 风 ,能 够 捕捉 声 源 中 的 有 效 信 息 , 可 用 于 语 
音 识别 领域 。 

由 于 Kinect 具有 以 上 的 设备 装置 , 它 主要 用 于 3 个 技术 领域 : 深度 识别 (3D 图 像 识 别 
技术 )、 人 体 骨 骼 追踪 技术 (动作 捕 提 技术)、 语 音 识 别 技 术 。 

目前 ,Kinect 设备 在 医学 领域 中 可 用 于 无 菌 化 操作 的 临床 手术 和 远程 医疗 手术 等 工 
作 ,在 商业 领域 中 可 用 于 购物 的 虚拟 试 衣 技术 ,在 机 器 人 领域 中 用 于 根据 人 的 肢体 动作 和 声 
音 控制 机 器 人 。Kinect 主要 用 于 捕捉 人 体 的 骨架 ,并 把 相关 的 数据 传送 到 计算 机 的 应 用 程 
序 中 ,用 于 与 虚拟 环境 进行 实时 的 交互 。 


3.12.2 交互 相关 软件 的 作用 


1. OpenNI 在 人 机 交互 中 的 作用 
OpenNICOpen Natural Interaction ,开放 式 自 然 交 互 ) 为 自然 交互 提供 了 编写 程序 的 应 
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程序 界面 ,从 而 定义 了 一 个 支持 多 种 编程 语言 . 跨 多 个 操作 平台 的 架构 。OpenNI 的 主要 
作用 是 构建 了 一 个 标准 式 的 应 用 程序 界面 ,这 个 界面 能 够 与 视觉 传感器 和 音频 传感器 以 及 
它们 的 感知 中 间 件 进行 通信 。 因 此 ,这 个 应 用 程序 界面 支持 的 交互 方式 有 语音 和 语音 命令 
识别 .手势 .身体 运动 跟踪 ,从 而 给 用 户 的 程序 提供 了 一 条 访问 和 使 用 这 些 具有 某 种 交互 功 
能 的 自然 交互 设备 。OpenNI 的 特性 使 得 用 户 在 开发 新 的 视觉 算法 或 使 用 新 的 传感器 时 ， 
极 大 地 减少 了 应 用 程序 的 开发 时 间 。 

2. NITE 在 人 机 交互 中 的 作用 

NITE( Natural Interaction Technology for End-user, 终 端 用 户 的 自然 交互 技术 ) 是 一 
个 感知 3D 世界 的 中 间 件 ,其 中 的 感知 原理 是 借助 于 传感器 深度 图 像 ,感知 人 类 所 做 的 动 
作 , 并 把 这 些 感知 以 同样 的 方式 转化 为 包含 这 些 信息 的 数据 。 因 此 ,一 个 3D 传感器 将 观察 
使 用 者 的 世界 (使 用 者 的 身体 以 及 他 的 动作 ) ,而 NITE 则 作为 感知 引擎 的 角色 ,用 于 理解 环 
境 中 的 这 些 用 户 交 互 方式 。NITE 既 包 含 了 用 于 识别 用 户 和 跟踪 他 们 的 动作 的 计算 机 视觉 
算法 ,也 包含 了 一 个 能 够 实现 自然 互动 界面 控件 的 应 用 程序 界面 ,这 些 界面 控件 是 基于 用 户 
手势 的 。 

3. SensorKinect 在 人 机 交互 的 作用 

SensorKinect 是 Kinect 的 驱动 程序 ,为 Kinect 与 计算 机 之 间 的 连接 提供 了 一 条 通道 ， 
能 够 将 Kinect 获取 的 外 界 图 像 数 据 传输 到 计算 机 的 其 他 程序 中 ,供用 户 进行 应 用 程序 
开发 。 

当 安装 好 SensorKinect 并 正确 连接 上 交互 设备 后 ,计算 机 中 的 设备 管理 器 将 会 出 现 
PrimeSensor, 包 括 了 KinectWindows Audio 和 KinectWindows Camera。 

4. Kinect 开发 环境 的 架构 

对 以 上 三 个 程序 进行 安装 后 ,完成 了 Kinect 开发 环境 的 搭建 。 如 图 3-135 Bron ,该 开 
发 环境 的 架构 可 以 分 为 3 层 : 程序 层 .OpenNI 界面 层 和 硬件 层 。 最 上 层 为 程序 层 , 机 器 人 
仿真 软件 V-REP 可 以 借用 OpenNI 界面 层 获取 的 信息 进行 相应 的 开发 使 用 ; 第 二 层 为 
OpenNI 界面 层 , 它 是 由 OpenNI 和 NITE 共同 提供 的 交互 接口 ,它们 定义 了 4 个 功能 主 件 ， 
即 全 身分 析 、 手 部 分 析 .手势 检测 和 场景 分 析 ; 最 底层 为 硬件 层 , 在 本 书 中 ,传感器 设备 主要 
是 Kinect XBOX 360, 它 通过 摄像 功能 能 够 获取 外 界 的 图 像 信 息 , 主 要 是 捕 提 人 体 的 动作 ， 
然后 通过 本 身 的 驱动 SensorKinect 与 计算 机 进行 连接 ,将 获取 的 图 像 信 息 传输 到 OpenNI 
界面 层 中 。 


3.12.3. 交互 相关 软件 的 安装 与 测试 


Kinect 开发 环境 的 搭建 需要 对 以 上 的 相关 软件 进行 正确 的 安装 与 配置 ,其 中 OpenNI 
与 NITE 版 本 的 位 数 需 要 与 用 户 计算 机 的 位 数 一 致 :而 二 者 之 间 的 版 本 也 需要 对 应 。 
OpenNI 采 用 的 是 32 位 ,序号 为 1. 5.7 的 版 本 ; NITE 采用 的 是 32 位 ,序号 为 1. 5.2 的 版 
本 。 安 装 过 程 中 需 注意 不 能 将 Kinect 与 计算 机 连接 ,而 且 以 上 3 个 软件 的 安装 顺序 依次 
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] 


(传感器 ) 


由 于 | 硬件 层 


图 3-135 ”OpenNI 开发 环境 的 三 层 结构 


为 : OpenNI, NITE, SensorKinect, 按 照 默认 路 径 进行 安装 。 

完成 以 上 的 安装 步骤 后 ,对 计算 机 进行 重启 ,将 会 使 已 经 改变 的 环境 生效 。 这 时 连接 上 
Kinect 后 ,可 以 对 已 搭建 的 开发 环境 进行 测试 ,看 是 否 已 经 正确 安装 。 此 时 ,选择 OpenNI 
或 NITE 安装 路 径 中 Sample 子 目录 中 的 例子 进行 测试 ,可 以 成 功 搭建 OpenNI 十 NITE 十 
SensorKinect 的 开发 环境 。 


3.12.4 OpenNIZNITE 中 的 人 体 骨 架 分 析 


在 OpenNI 中, 人体 骨 架 主 要 是 由 关节 构成 的 ,而 每 一 个 关节 都 具备 有 一 个 位 置 和 一 个 
姿态 。 在 OpenNI 中 ,一 共 定 义 了 人 体 的 24 个 关 
节 , 但 通过 NITE 这 个 中 间 件 来 分 析 人 体 骨 架 时 ， 
如 图 3-136 所 示 , 只 能 用 到 以 下 15 个 关节 : 头 部 、 过 
脖子 ECT rro ACOR oi CIR or COP df 
CIE A CAO MERGE CIO M 

通过 OpenNI/ NITE 来 获取 追踪 人 体 骨 架 的 
过 程 如 图 3-137 所 示 , 图 中 第 一 个 方块 代表 用 户 产 
生 器 , 它 包含 着 两 个 事件 : 产生 新 用 户 和 丢失 用 
户 。 产 生 新 的 用 户 这 个 事件 会 在 “可 侦 测 的 范围 内 图 3-136 NITE 中 的 人 体 骨 架 图 
检测 到 新 的 使 用 者 ”时 被 创建 ; 丢失 用 户 会 在 “使 
者 离开 了 画面 已 有 一 段 时 间 ” 时 被 创建 。 第 二 个 方块 是 位 置 检测 。 在 用 户 产生 器 侦 测 到 
有 新 的 使 用 者 时 ,这 个 方块 会 使 用 预先 定义 的 校正 姿态 去 检测 。 当 使 用 者 摆 出 这 个 预先 定 
义 的 姿态 时 ,位 置 检测 方块 一 方面 将 会 结束 检测 , 另 一 方面 将 启用 第 三 个 方块 。 当 第 三 个 方 
块 的 需要 校准 模块 被 创建 时 ,就 会 开始 对 骨架 进行 校正 和 分 析 。 校 准 成 功 后 ,将 会 停止 这 个 
工作 ,并 成 功 地 追踪 到 人 体 骨 架 ,最 后 成 功 地 读 取 到 最 新 的 骨架 数据 。 
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^ 
用 户 产生 器 


人 体 骨架 


位 置 检测 


已 经 检测 
到 位 置 


检测 位 置 f 


结束 校准 


用 户 丢失 成 功 读 取 成 功 追 踪 
骨架 数据 人 体 骨 架 
及 二 一 水 = 一 


3-137. 建立 人 体 骨架 的 流程 


3.12.5 V-REP 与 Kinect 接口 的 安装 与 测试 


对 Kinect 的 开发 环境 搭建 后 ,需要 安装 V-REP 与 Kinect 之 间 的 接口 ,此 时 需要 经 过 两 
个 步骤 : 

CD 相关 插件 的 安装 : 在 OpenNI 和 NITE 的 安装 路 径 子 目录 中 寻找 以 下 3 个 文件 : 
glut32. dll, OpenNI. dll 和 SamplesConfig. Xml ,并 把 这 些 文件 复制 到 V-REP 的 安装 路 径 中 
(默认 路 径 为 C:\Program Files (x86)\V-REP3\V-REP_PRO_EDU)。 它 们 的 作用 是 : 为 
V-REP 与 Kinect 二 者 之 间 的 交互 提供 相关 的 插件 。 

(2) KinectSever 的 重 编译 : 作为 开源 的 机 器 人 仿真 软件 ,V-REP 在 其 安装 文件 夹 中 提 
供 了 一 个 KinectSever. exe 以 及 它 的 C++ 源 代码 。 但 由 于 V-REP 及 其 他 相关 软件 版 本 的 不 
同 ,该 程序 不 能 被 直接 应 用 ,需要 对 KinectSever 进行 重 编译 。 

重 编译 的 大 致 过 程 是 : 首先 将 OpenNI 的 Include 中 的 C++ 头 文件 复制 到 V-REP 提供 
的 KinectSever 源 代码 所 在 的 文件 夹 中 。 然 后 利用 Visual Studio 2010 对 源 代码 进行 编译 
(其 他 比较 新 的 Visual Studio 版 本 存在 不 兼容 的 问题 ) ,对 相关 代码 存在 的 bug 进行 修改 调 
试 后 ,直到 单 击 生 成 解决 方案 ,能 够 生成 一 个 新 的 KinectSever. exe。 最 后 需要 把 新 生成 的 
程序 复制 到 V-REP 的 安装 文件 夹 中 ,替换 旧 的 程序 。 
正确 完成 了 以 上 开发 环境 及 接口 的 搭建 后 ,需要 进行 相关 的 测试 。 在 V-REP 提供 的 
模型 中 ,有 一 个 名 为 “interface to kinect. ttm” 的 模型 ,可 以 使 用 它 进行 测试 。 如 图 3-138 所 
示 , 测 试 的 结果 如 下 : 

(1) 在 模型 浏览 器 中 ,将 “interface to kinect. ttm” 加 载 到 空白 的 场景 中 ,这 时 场景 中 出 
现 了 一 个 小 球 。 

(2) 运行 仿真 ,如 果 以 上 的 步骤 都 能 正确 进行 ,将 自动 加 载 程序 KinectSever. exe, 弹 出 
通过 Kinect 感知 外 界 图 像 的 窗口 。 否 则 ,将 会 弹出 相对 应 的 错误 提示 ,如 “无 法 启动 此 程 
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图 3-138 V-REP 通过 Kinect 获取 的 人 体 骨架 


序 , 因 为 计算 机 丢失 glut32. dll", 

(3) 使 用 者 站 在 Kinect 的 前 方 一 定 距离 ,其 手势 的 运动 将 会 被 Kinect 所 捕捉 到 。 此 
时 ,如 图 所 示 ,V-REP 场景 中 的 小 球 变 成 人 的 骨架 ,由 若干 个 点 组 成 。 使 用 者 在 进行 运动 
时 ,场景 中 的 人 形 骨 架 也 跟随 着 运动 。 


3.12.6 Kinect 与 V-REP 交互 的 设计 与 实现 


1. Kinect 与 V-REP 交互 的 设计 

通过 “interface to kinect. ttm” 的 成 功 测试 ,表明 了 Kinect 与 V-REP 已 经 实现 了 连接 。 
此 时 ,为 进一步 研究 二 者 之 间 更 友好 自然 的 交互 方式 ,设计 一 个 预期 的 实验 目标 : 通过 
Kinect 捕 提 人 的 动作 ,从 而 控制 V-REP 中 人 形 机 器 人 的 运动 。 

V-REP 模型 库 提供 的 Asti 机 器 人 如 图 3-139 所 示 ,Asti 可 活动 的 关节 部 分 主要 由 膀 
子 、 左 辟 \ 右 辟 、 左 腿 、 右 腿 五 个 部 分 组 成 ,在 这 各 大 部 分 中 由 具有 许多 个 关节 ,一 共有 20 个 
关节 。 借 鉴 *interface to kinect, ttm” 中 对 数据 的 写 入 及 读 取 的 方法 进行 编程 ,而 且 上 文 提 
到 OpenNI/NITE 对 人 体 骨 架 的 捕 提 只 有 15 个 关节 点 ,因此 不 需 利用 到 全 部 的 关节 点 。 


Be 


n 加 而 
4 esti body 


AREETA TEE EE] 


BABAS 


图 3-139 V-REP 中 的 Asti 机 器 人 及 其 层次 结构 


352 4| 机 器 人 仿真 与 编程 技术 


V-REP 中 的 interface to kinect. ttm 实现 了 从 Kinect 读 取 人 体 的 骨架 点 ,并 把 它 投影 


到 场景 


Ph。 在 相对 的 Lua 嵌入 式 脚本 中 , 它 的 主要 工作 流程 如 下 : 


@ 启用 了 一 个 服务 器 端口 ,并 建立 了 一 个 套 接 字 ,并 让 它 连接 到 KinectSever 服务 端 ; 

Q 定义 了 一 个 写 数据 至 套 接 字 的 函数 : 首先 计算 了 需要 发 送 多 少 个 数据 包 , 然 后 进行 
数据 的 写 入 ; 

© 定义 了 一 个 从 套 接 字 读 取 数 据 的 函数 ; 

CD 准备 画 线 和 显示 球状 ,用 于 显示 人 体 的 骨架 。 

将 Asti 机 器 人 加 载 到 空白 的 场景 中 ,并 对 模型 进行 一 定 的 修改 ,给 它 添加 了 一 个 非 线 程 
的 嵌入 式 脚 本 ,然后 根据 以 上 的 方法 进行 Lua 代码 的 编写 ,主要 补充 的 核心 部 分 是 将 V-REP 
获取 的 人 体 骨架 点 赋予 Asti 机 器 人 相对 应 的 关节 。 

相关 的 Lua 代码 为 : 


Following function writes data to the socket (the data might be sent in several packets) 


writeSocketData = function(client, data) 


—— Check how many packets we need to send: 
local packetCount - 0 
local s= # data 
while (s~ = 0) do 
packetCount = packetCount + 1 
if (s>256-6) then —— this is the max packet size minus header size 


s=s-256+6 
else 
s=0 
end 
end 
—- Now send the data: 
s= #data 


local pointer = 0 
while (s~ = 0) do 
packetCount = packetCount 一 1 
local sizeToSend = s 
if (s>256-6) then 
sizeToSend = 256 - 6 
end 
s= s- sizeToSend 
local header = string. char (59, 57, math. mod ( sizeToSend, 256), math. floor (sizeToSend/ 


256), math. mod( packetCount, 256) , math. floor ( packetCount/256) ) 


end 


Packet header is: headerID (59,57), dataSize (WORD), packetsLeft (BYTE) 
client:send(header. . data: sub( pointer + 1, pointer + sizeToSend)) 
pointer = pointer + sizeToSend 

end 
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—— Following function reads data from the socket (that might be arriving in several packets) 
readSocketData - function(client) 
local returnData- '' 
while (true) do 
—- Packet header is: headerID (59,57), dataSize (WORD), packetsLeft (WORD) 
local header = client:receive(6) 
if (header == nil) then 
return(nil) -- error 
end 
if (header:byte(1) == 59)and(header:byte(2) == 57) then 
local 1 = header:byte(3) + header:byte(4) * 256 
returnData = returnData. .client:receive(l) 
if (header:byte(5) == 0)and(header:byte(6) == 0) then 
break -- That was the last packet 
end 
else 
return(nil) -- error 
end 
end 
return(returnData) 
end 


linkPoints = function(returnData, indexl, index2, minConf idence) 
if (returnData[4 * indexl + 4] > minConfidence) and (returnData [4 * index2 + 4]> 
minConfidence) then 
local data = (returnData[4 * indexl + 1], returnData[4 * indexl + 2], returnData[4 * 
indexl + 3], returnData[4 * index2 + 1], returnData[4 * index2 + 2], returnData[4 * index2 + 3]) 
data[1] = data[1]- 1 
data[4] = data[4] - 1 
simAddDrawingObjectlItem(lineContainer, data) 
end 
end 


threadFunction = function() 
while simGetSimulationState()— = sim simulation advancing abouttostop do 

—- Send a request to the server (just anything): 

writeSocketData(client, ' ') 

—— Read the reply from the server: 

local returnData - readSocketData(client) 

if (returnData == nil) then 
break -— Read error 

else 
returnData = simUnpackFloats(returnData) 
simAddDrawingObjectltem(lineContainer, nil) 
simAddDrawingObjectItem(sphereContainer, nil) 
torsoTransf = simGetObjectMatrix(objectHandle, - 1) 
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if (returnData[60]> 0.5) then 
torsoPos = {returnData[57]/1000,returnData[58]/1000,returnData[59]/1000} 
torsoPos = sinMultiplyVector(m, torsoPos) 
end 
for i- 0,15,1 do 
if (i«6)or(i»13) then 
if (returnData[4 » i+4]> 0.5) then 
SscalingFact = 0.7 


pointPos = (returnData[4 * i+ 1] * scalingFact/1000, returnData[4 x i+ 2] * 
scalingFact/1000, returnData[4 * i- 3] * scalingFact/1000) 


pointPos = simMultiplyVector(m, pointPos) 
pointPos[1] = pointPos[1] - torsoPos[1] 
pointPos[2] = pointPos[2] - torsoPos[2] 
pointPos[3] = pointPos[3] - torsoPos[3] 

pointPos = simMultiplyVector(torsoTransf, pointPos) 
returnData[4 * i+ 1] = pointPos[1] 

returnData[4 x i * 2] = pointPos[2] 

returnData[4 * i * 3] = pointPos[3] 

pointPos[1] = pointPos[1] - 1 

—- Skeleton animation in V- REP 
simAddDrawingObjectItem( sphereContainer, pointPos) 


end 


local packedData 7 simPackFloats(returnData) 
simSetStringSignal( 'MyJointData', packedData) 


linkPoints(returnData, 0, 2,0.5) 
linkPoints(returnData,1,3,0.5) 
linkPoints(returnData,5,14,0.5) 
linkPoints(returnData,4,14,0.5) 
linkPoints(returnData,2,4,0.5) 
linkPoints(returnData,3,5,0.5) 
linkPoints(returnData,4,5,0.5) 
linkPoints(returnData,4,15,0.5) 
linkPoints(returnData,5,15,0.5) 


if (returnData[4 * 0 + 4]» 0. 5) and(returnData[4 x 1 * 4]> 0. 5) and(returnData[4 x 
2*4]» 0. 5)and(returnData[4 * 3 + 4]» 0. 5)and(returnData[4 * 4 + 4]» 0. 5)and(returnData[4 * 5 + 
4]> 0.5)and(returnData[4 * 14 + 4]> 0. 5) then 
pt1 = (returnData[4 * 4 + 1], returnData[4 * 4 + 2], returnData[4 x 4 * 3]) 
pt2 = (returnData[4 + 5 + 1], returnData[4 x 5 + 2], returnData[4 x 5 * 3]) 


pt3 = (returnData[4 * 14 + 1], returnData[4 x 14 + 2], returnData[4 x 14 +3]} 
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vl = (pt1[1] - pt2[1],pt1[2] - pt2[2], pt1[3] - pt2[3]} 
v2 = (pt3[1] - pt2[1], pt3[2] - pt2[2], pt3[3] - pt2[3]} 


n= (v1[2] * v2[3] - v1[3] * v2[2], v1[3] x v2[1] - v1[1] * v2[3], v1[1] x v2[2] - 
v1[2] * v2[1]) 
l= math. sqrt(n[1] x n[1] * n[2] * n[2] * n[3] * n[3]) 
n[1]7 n[1]/1 
n[2] = n[2]/1 
n[3] 2 n[3]/1 
dd- (0,0,1.5,n[1],n[2],1.5 * n[3]) 
correctionAngle - math. asin(n[3]) 


z= (v1[2] * [3] - v1[3] * n2], v1[3] * n[1] - v1[1] * [3], vi[1] * n[2] - 
v1[2] * n[1]) 
l= math. sart(z[1] * z[1] + z[2] * z[2] * z[3] * z[3]) 
z[1] = z[1]/1 
z[2] = z[2]/1 
z[3] = z[3]/1 
dd= (0,0,1.5,2[1],2[2], 1.5 + z[3]] 


x= (n[2] * z[3] - n[3] * z[2],n[3] x z[1] - n[1] x z[3],n[1] * z[2] - nI2] « z[1]) 
dd= (0,0,1.5,x[1],x[2],1.5 + x[3]) 
lsp = simGetObjectPosition(leftShoulder, - 1) 
rsp = simGetObjectPosition(rightShoulder, — 1) 
mm= (0,0,0,0,0,0,0,0,0,0,0,0) 

mm[1] = x[1] 

mm[2] =n[1] 

mm[3] = z[1] 

mm[4] = returnData[17] 

mm[5] = x[2] 

mm[6] = n[2] 

mm[7] = z[2] 

mm[8] = returnData[18] 

mm[9] = x[3] 

mm[10] = n[3] 

mm[11] = z[3] 

mm[12] = returnData[19] 

mml = simGetInvertedMatrix(mm) 

mm[4] = returnData[21] 

mm[8] = returnData[22] 

mm[12] = returnData[23] 

mmr = simGetInvertedMatrix(mm) 


leftHandP = simMultiplyVector(mml, (returnData[1], returnData[ 2], returnData[3]}) 
leftHandP[1] = leftHandP[1] + 1sp[1] 
leftHandP[2] = leftHandP[2] + 1sp[2] 
leftHandP[3] = leftHandP[3] + 1sp[3] 


rightHandP- simMultiplyVector(mmr, (returnData[5], returnData[ 6], returnData[ 7]] ) 
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rightHandP[1] = rightHandP[1] + rsp[1] 
rightHandP[2] = rightHandP[2] + rsp[2] 
rightHandP[3] = rightHandP[ 3] + rsp[3] 


leftElbowP = simMultiplyVector(mml, (returnData[9], returnData[10], 
returnData[11])) 

leftElbowP[1] = leftElbowP[1] + 1sp[1] 

leftElbowP[2] = leftElbowP[2] + 1sp[2] 

leftElbowP[3] = leftElbowP[3] + 1sp[3] 


rightElbowP = simMultiplyVector(mmr, (returnData[13], returnData[14], 
returnData[15]]) 
rightElbowP[1]- rightElbowP[1] + rsp[1] 
rightElbowP[2] = rightElbowP[2] + rsp[2] 
rightElbowP[3] = rightElbowP[3] + rsp[3] 
simSetObjectPosition(leftHand, - 1, leftHandP) 
simSetObjectPosition(rightHand, - 1,rightHandP) 
sinSetObjectPosition(leftElbow, — 1, leftElbowP) 
simSetObjectPosition(rightElbow, — 1, rightElbowP) 
end 
end 
sinSwitchThread() 
end 
end 


—- Put some initialization code here: 
simSetThreadSwitchTiming(200) -- We wanna manually switch for synchronization purpose (and 
also not to waste processing time!) 

if (simGetInt32Parameter(sim intparam platform) 


0) then —— for now only Windows is supported 


—- We start the server on a port that is probably not used (try to always use a similar 一 一 
code): 
simSetThreadAutomaticSwitch(false) 
local portNb = simGetInt32Parameter(sim intparam server port next) 
local portStart - simGetInt32Parameter(sim intparam server port start) 
local portRange = simGetInt32Parameter(sim intparam server port range) 
if (not portNb) then 
portNb = portStart 
end 


local newPortNb = portNb + 1 
if (newPortNb > = portStart + portRange) then 
newPortNb - portStart 
end 
simSetInt32Parameter(sim intparam server port next, newPortNb) 
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simSetThreadAutomaticSwitch(true) 
simLaunchExecutable( 'kinectServer. exe', portNb, 1) 


—- Build a socket and connect to the server: 

Socket = require("socket" ) 

client - socket. tcp( ) 

simSetThreadIsFree(true) —- To avoid a bief moment where the simulator appears as locked 
local result = client:connect( '127.0.0. 1', portNb) 

simSetThreadIsFree( false) 


—- Prepare the drawing containers for lines and spheres (to display the skeleton): 
lineContainer = simAddDrawingObject(sim drawing lines,4,0, — 1,100,(1,0,0]) 


sphereContainer = simAddDrawingObject(sim drawing spherepoints,0.05,0, - 1,100,(0,0,1]) 
objectHandle - simGetObjectHandle( 'astiTorso') 

leftHand = simGetObjectHandle( 'leftHandSphere') 

rightHand = simGetObjectHandle('rightHandSphere') 

leftElbow = simGetObjectHandle('leftElbowSphere') 

leftShoulder = simGetObjectHandle( 'leftShoulderSphere') 

rightShoulder = simGetObjectHandle( 'rightShoulderSphere') 


m= sinBuildMatrix((0, 0, 0), (nath. pi/2,0,0] ) 

objectHandle = simGetObjectAssociatedWithScript(sim handle self) 
torsoPos = {0,0,0} 

scalingFact = 0.7 

w=1.2 

correctionAngle = 0 


if (result == 1) then 
—- We could connect to the server 


—- Here we execute the regular thread code: 
res, err = xpcall(threadFunction, function(err) return debug. traceback(err) end) 
if not res then 
simAddStatusbarMessage('Lua runtime error: 
end 


..err) 


end 

client:close() 
else 

simDisplayDialog('Error', 'This model is not yet supported on this platform and currently 
only works on Windows. &&nContact us for source code examples if you wish to also have support on 
this platform. ', sim dlgstyle ok,true) 
end 
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2. Kinect 与 V-REP 交互 的 实现 效果 

CD 仿真 开始 时 ,将 自动 运行 KinectSever. exe, 通 过 该 程序 与 OpenNI/ NITE 的 开发 环 
境 进行 通讯 。 其 效果 是 弹出 一 个 Kinect 捕 提 图 像 信息 的 窗口 , 当 人 体 距 离 Kinect 一 定 的 距 
离 时 ,将 会 自动 辨识 并 追踪 人 体 , 并 把 人 体 的 颜色 标注 为 蓝 色 。 

(2) 当 人 体 远 离 Kinect 所 能 捕捉 的 视觉 范围 后 ,再 次 进入 该 范围 时 ,人 体 的 颜色 开始 
依然 为 灰色 ,只 有 等 到 被 再 次 辨识 出 来 才 会 变 成 蓝 色 。 当 Kinect 已 经 辨识 出 人 体 时 ,此 时 
视觉 范围 中 出 现 另 一 个 人 体 或 其 他 干扰 物 ,将 会 被 标注 成 其 他 颜色 。 

(3) 当 人 体 再 向 空旷 的 后 方 移动 时 ,图 像 窗 口 将 会 出 现 人 的 骨架 点 。 此 时 ,如 图 3-140 
所 示 , 人 体 开始 动作 时 ,V-REP 中 的 Asti 机 器 人 将 会 跟随 人 体 双 手 的 动作 进行 实时 地 运 
动 。 目 前 ,能 够 实现 Asti 机 器 人 跟随 人 体 进行 举 起 手臂 、 弯 曲 手 臂 和 使 手臂 垂下 等 动作 。 


图 3-140 ”使 用 Kinect 控制 V-REP 中 的 Asti 机 器 人 


以 上 的 实验 结果 表明 ,使 用 者 能 够 通过 Kinect 控制 V-REP 中 的 机 器 人 ,让 它 完成 一 定 
的 动作 ,因此 通过 Kinect 进行 虚拟 人 机 交互 的 研究 与 设计 能 够 成 功 实现 。 


本 章 小 结 


本 章 介 绍 了 V-REP 机 器 人 仿真 的 基础 知识 ,包括 了 它 的 安装 、 用 户 界面 ,场景 与 模型 、 
计算 模块 和 控制 方法 等 ,然后 介绍 了 如 何 使 用 V-REP 进行 仿真 , 它 的 主要 步骤 为 : 模型 搭 
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建 \ 编 程 实现 控制 方法 ,设置 仿真 。 最 后 介绍 了 V-REP 在 人 机 交互 的 应 用 ,包括 了 V-REP 如 
何 和 Touch X、Kinect 之 间 进 行 通信 。 
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Gazebo 在 机 器 人 仿真 中 的 应 用 


4.1 Gazebo 的 介绍 与 安装 


4.1.1 Gazebo 的 初步 介绍 


Gazebo 是 一 款 机 器 人 3D 动力 学 仿真 软件 , 它 能 够 精确 并 且 有 效 地 对 在 室内 或 室外 环 
境 中 一 定数 量 的 机 器 人 进行 3D 动力 学 仿真 。 跟 V-REP 等 其 他 机 器 人 仿真 软件 相似 ， 
Gazebo 能 够 提供 高 精确 度 的 物理 仿真 。 除 此 之 外 ,Gazebo 还 提供 了 一 系列 的 传感器 ,针对 
使 用 者 和 程序 的 接口 。 

Gazebo 具有 丰富 的 功能 ,其 中 典型 应 用 有 : 机 器 人 算法 测试 、 机 器 人 设计 、 机 器 人 现实 
可 能 情况 的 回归 分 析 。 

Gazebo 功能 丰富 ,具有 多 种 特征 ,其 中 一 些 关 键 的 特征 包括 : 

。 Gazebo 拥有 大 量 物理 引擎 ,包括 了 ODE, Bullet, Simbody fll DART; 

。 Gazebo 具有 一 个 丰富 的 机 器 人 库 和 环境 库 ; 

。 Gazebo 具有 各 种 类 型 的 传感器 ; 

。 具 有 便利 的 可 编程 图 表 化 接口 。 


4.1.2 Gazebo 的 安装 


1. Ubuntu 下 使 用 Ubuntu 包 安 装 Gazebo 
CD 默认 安装 : 如 果 需 要 进行 Gazebo 版 本 的 默认 安装 ,输入 以 下 命令 : 


curl - ssL http://get.Gazebosim.org | sh 


(2) 可 选择 安装 。 
设置 用 户 的 电脑 可 以 接受 从 packages. osrfoundation. org 下 载 的 软件 ,然后 输入 以 下 命令 ， 


sudosh - c 'echo "deb http://packages. osrfoundation. org/Gazebo/ubuntu - stable '1sb_release 
— cs' nain" > /etc/apt/sources. list.d/Gazebo - stable. list' 


第 4 章 ”Gazebo 在 机 器 人 仿真 中 的 应 用 | 361 


用 户 可 以 检查 该 文件 是 否 书 写 正确 ,在 Ubuntu 的 终端 中 ,用 户 可 以 输入 以 下 命令 进行 
安装 : 


$ cat /etc/apt/sources. list. d/Gazebo - stable.list 
deb http: //packages. osrfoundation. org/Gazebo/ubuntu - stable trusty main 


安装 关键 的 软件 包 : 


wget http://packages. osrfoundation. org/Gazebo.key -0 - | sudo apt -key add — 
install Gazebo 


首先 更 新 debian 数据 库 : 


sudo apt - get update 


注意 : 确保 apy-get 更 新 过 程 没有 错误 ,控制 台 输出 以 done 结束 ,类 似 以 下 输出 : 
$ sudo apt - get update 


hit http: //ppa. launchpad. net trusty/main Translation - en 

ign http: //us. archive. ubuntu. com trusty/main Translation- en US 

ign http: //us. archive. ubuntu. com trusty/multiverse Translation- en US 
ign http: //us. archive. ubuntu. com trusty/restricted Translation- en US 
ign http: //us. archive. ubuntu. com trusty/universe Translation - en US 
reading package lists... Done 


然后 通过 下 列 命令 安装 Gazebo8: 


sudo apt - get install Gazebo8 
# for developers that work on top of Gazebo, one extra package 
sudo apt - get install libGazebo8 - dev 


如 果 用 户 看 到 如 下 错误 : 


$ sudo apt- get install Gazebo8 
reading package lists... Done 
building dependency tree 

reading state information... Done 
E: Unable to locate package Gazebo8 


产生 错误 的 原因 是 Gazebo 的 版 本 中 用 户 的 操作 系统 版 本 不 匹配 。 例 如 在 Ubuntul4. 04 
上 安装 Gazebo8 会 出 现 上 述 错误 。 注 意 : 通过 查看 “Project Status” 可 查看 每 个 Gazebo X 
持 的 Ubuntu 版 本 和 ROS 版 本 。 此 时 ,你 将 查看 自己 的 系统 版 本 ,然后 安装 相对 应 的 
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Gazebo 版 本 。 完 成 以 上 的 安装 步骤 后 .输入 以 下 的 命令 运行 Gazebo, 测 试 安装 是 否 正确 : 


者 。 


Gazebo 


注意 : Gazebo 第 一 次 运行 需要 下 载 一 些 模型 ,这 将 花费 一 些 时 间 ,请 耐心 等 候 。 

(3) Gazebo 中 不 同 的 debian 安装 包 。 

Gazebo 中 有 以 下 两 种 不 同 的 debian 安装 包 : 

O 把 Gazebo 作为 应 用 程序 : 针对 只 使 用 Gazebo 仿真 软件 提供 的 插件 ,模型 的 使 用 
不 打算 在 自己 的 软件 中 进行 Gazebo 顶层 设计 。 为 使 用 Gazebo8 ,请 安装 Gazebo8 。 

© 使 用 Gazebo 的 库 来 开发 软件 : 开发 插件 或 者 任意 软件 需要 Gazebo 的 头 文件 或 库 。 


在 这 种 情况 下 ,请 在 下 载 Gazebo8 的 基础 上 下 载 libGazebo8-dev。 


2. 在 Windows 下 安装 
安装 过 程 需要 使 用 本 地 预 编译 好 的 二 进 制 文件 。 为 了 使 安装 更 加 简单 ,使 用 MinGW 


来 进行 编辑 工作 (如 GitBash Shell) ,并 且 只 使 用 Windows 的 命令 行 工 具 来 配置 和 构建 ,用 
户 可 能 需要 禁用 Windows 防火 墙 。 具 体 的 安装 步骤 如 下 : 


(D 创建 工作 目录 ,输入 以 下 命令 : 


mkdirgz— ws 
cdgz—ws 


© 下 载 如 下 附件 到 该 目录 里 面 : 

freeImage 3. x. slightly modified to build on VS2013 

* boost 1. 56.0 

* bzip2 1.0.6 

dlfcn-win32 

ibcurl HEAD 

* OGRE 1.9.0 rcl 

* protobuf 2.6.0 

TBB 4. 3 

ziplib 0. 13.62 

e zlib 

Q 在 gz-ws 下 解压 ; 

®© 下载 Qt 4.8, 解 压 到 C:\Qt\4. 8. 6\x64\msvc2013; 

© 安装 cmake, 确 保 选择 “为 所 有 使 用 者 添加 CMake 系统 路 径 ” 选 项 ; 
© 安装 Rubyl.9 或 更 高 版 本 ,安装 过 程 确保 添加 Ruby 到 系统 路 径 ; 
CD 复制 Ignition Math, Sdformat 和 Gazebo; 


第 4 章 ”Gazebo 在 机 器 人 仿真 中 的 应 用 | 363 


hg clone https://bitbucket. org/ignitionrobotics/ign - math 
hg clone https://bitbucket. org/osrf/sdformat 
hg clone https://bitbucket. org/osrf/Gazebo 


打开 命令 窗口 ,通过 如 下 命令 加 载 编译 器 进行 安装 : 


"C:\Program Files (x86)\Microsoft Visual Studio 12. 0\VC\vcvarsall. bat" x86 amd64 


© 配置 和 构建 Ignition Math; 


cdign - math 

mkdir build 

cd build 

# if you want debug, run . .\configure Debug 
.. \conf igure Release 

nmake 

nmake install 


此 时 用 户 应 该 在 gz-ws/ign-math/build/install/Release 安装 了 Ignition Math, 
D 在 同一 个 窗口 下 ,配置 和 安装 Sdformat: 


cd .. V. . Nsdformat 

mkdir build 

cd build 

# if you want debug, run ..Vconfigure Debug 
.. \conf igure 

nmake 

nmake install 


此 时 , 在 gz-ws/sdformat/build/install/Release 或 gz-ws/sdformat/build/install/ 
Debug 下 安装 了 Sdformat。 
D 在 相同 的 命令 窗口 下 ,配置 和 构建 Gazebo: 


cd . .\. . \Gazebo 

mkdir build 

cd build 

# if you want debug, run ..\configure Debug 
. .\conf igure 

nmakegzclient 

nmakegzserver 

nmake install 


此 时 应 在 gz-ws/Gazebo/ build/install/Release 或 gz-ws/Gazebo/ build/install/Debug 
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下 安装 了 Gazebo。 为 了 在 Windows 环境 下 使 用 Gazebo 的 所 有 功能 ,还 需要 进行 以 下 
首先 ,运行 : 


gzserver 


调整 所 有 路 径 以 加 载 dll: 
(D 如 果 在 Debug rp Jy; 


cdgz - ws V Gazebo V build .. \ win addpath. batDebug 


(2) 如 果 在 Release 中 ,为 : 


cdgz - ws \ Gazebo V build .. \ win addpath. batRelease 


(3) 创建 一 个 ogre plugins. cfg 文件 。 
(4) 输入 


cdgz — ws\Gazebo\build\ Gazebo 


(5) 如 果 在 Debug 中 : 复制 下 面 内 容 到 plugins. cfg and replace MYUSERNAME with 


your act ual username; 


* Define plugin folder 
PluginFolder = C:\Users\MYUSERNAME\ gz - wsVogre src vl - 8- 1 - vcl2 - x64 - release - debugV 
buildVinstallVDebugVbinVDebug 


* Define plugins 

Plugin = RenderSystem GL d 

Plugin- Plugin ParticleFX d 

Plugin- Plugin BSPSceneManager d 
Plugin- Plugin PCZSceneManager d 
Plugin- Plugin OctreeZone d 
Plugin- Plugin OctreeSceneManager d 


3. 如 果 在 Release 中 : 复制 下 面 内 容 到 plugins. cfg 


井 Define plugin folder 
PluginFolder = C:\Users\MYUSERNAME\ gz — wsVogre src vl- 8- 1 - vcl2 - x64 - release - debugV 
buildVinstallVReleaseVbinV Release 


# Define plugins 
Plugin = RenderSystem GL 
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Plugin = Plugin ParticleFX 
Plugin = Plugin BSPSceneManager 
Plugin = Plugin PCZSceneManager 
Plugin = Plugin OctreeZone 

Plugin = Plugin OctreeSceneManager 


将 此 文件 复制 到 gui 目录 中 ,输入 下 面 的 命令 : 

copyplugins. cfggui\ 

然后 ,运行 gzserver: 

gzserver. exe ..\. . \worlds\enpty. world 

接 下 来 ,调试 。 查 看 用 户 在 Gazebo 上 需要 调试 遇 到 的 问题 ,可 以 输入 : 
run gzserver 

如 果 遇 到 问题 ,请 使 用 -verbose 获取 更 多 信息 ,输入 


run gzclient 


一 个 已 知 的 问题 是 , 它 不 运行 在 Ubuntu 15. 04 主机 的 VirtualBox 3.4。 当 前 的 理论 是 
它 不 支持 离 屏 帧 缓冲 。 它 已 经 确认 工作 在 Windows 7 和 Ubuntu 14. 04 主机 的 VMWare 
播放 器 。 随 着 测试 的 继续 ,将 添加 更 多 的 细节 。 

接 下 来 构建 Ogre 示例 。 

(1) 下 载 OIS。 下 载 地 址 为 : 

http: //sunet. dl. sourceforge. net/project/wgois/Source% 20Release/1. 3/ois-v1-3. zip 

(2) 在 Visual Studio 中 编译 OIS 使 用 Win32 /文件 夹 中 的 项 目 。 

(3) 将 OIS 标题 和 库 放 入 : 


ogre —- ... V Dependencies V include ogre - ... V Dependencies V lib 
ogre - ... V Dependencies V bin 


(4) 补丁 ogre-1. 8 里面 的 configure. bat. fi H] : 


— DOGRE BUILD SAMPLES:BOOL = TRUE .. 


(5) 正常 编译 : 


. . \conf igure. bat 
nmake 
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(6) 使 用 以 下 命令 运行 演示 浏览 器 : 
# 将 0IS ”x* .dll 复制 到 bin 目录 ogre — ... V build \ bin V SampleBrowser.exe 中 . 


可 以 看 出 ,在 Windows 的 环境 下 安装 Gazebo 的 步骤 比较 多 ,这 里 不 推荐 用 户 在 
Windows 环境 下 使 用 Gazebo。 此 外 ,Gazebo 还 可 以 在 OS X 系统 中 安装 ,具体 的 步骤 可 以 
参考 Gazebo 的 官方 网 站 。 


4.1.3 Gazebo 与 V-REP 的 比较 


在 前 面 的 部 分 中 详细 地 介绍 了 机 器 人 仿真 软件 V-REP。 在 这 里 ,我们 将 这 两 个 常用 的 
仿真 软件 进行 了 比较 ,包括 以 下 四 个 部 分 : 

* ROS 集成 : 使 用 现 有 的 ROS 框架 与 仿真 软件 ,使 它们 一 起 工作 的 难点 。 

。 场景 建 模 : 创建 像 迷宫 一 样 的 场景 模型 的 难点 。 

。 机 器 人 模型 修改 : 修改 仿真 软件 提供 的 机 器 人 模型 ,在 机 器 人 上 添加 传感器 和 插件 

的 难点 。 

。 程序 控制 : 使 用 编程 语言 来 控制 仿真 环境 的 难点 。 

1. 与 ROS 之 间 的 集成 

Gazebo 是 ROS 系统 中 使 用 的 默认 机 器 人 仿真 软件 。Gazebo 与 ROS 是 分 离 的 项 目 ， 
即 它 们 的 开发 者 是 不 相同 的 ,但 有 一 个 与 Gazebo 相关 的 软件 包 在 ROS 官方 存储 库 (ros- 
indigo-gazebo-ros) 这 是 由 Gazebo 开发 者 自己 维护 的 软件 包 , 它 包含 接口 ROS 和 Gazebo 的 
插件 。 这 些 插件 可 以 连接 到 仿真 软件 场景 中 的 对 象 ,并 提供 简单 的 ROS 通信 方法 ,例如 
Gazebo 发 布 和 订阅 的 主题 以 及 服务 。 将 Gazebo 封装 为 ROS 节点 ,还 允许 将 其 轻松 集成 到 
运行 大 型 和 复杂 系统 ( 称 为 启动 文件 ) 的 ROS 默认 方法 中 。 

V-REP 中 没有 位 于 本 机 的 ROS 节点 。 这 意味 着 它 还 不 可 能 在 单个 启动 文件 中 作为 
ROS 系统 的 一 部 分 运行 ,而 在 另 一 个 Linux 终端 上 运行 它 。 另 一 方面 ,V-REP 提供 了 一 个 
默认 的 ROS 插件 ,可 以 在 VREP Lua 脚本 中 使 用 它 来 创建 ROS 发 布 者 和 订阅 者 (关于 V-REP 
与 ROS 之 间 的 插件 将 在 第 6 章 中 具体 介绍 ) 。 此 外 ,需要 ROS 与 V-REP 之 间 进 行 集成 ,更 
有 用 的 是 由 来 自 法 国 INRIA 研究 所 的 研究 组 Lagadic 创建 的 软件 包 ( 插 件 )。 这 个 软件 包 
在 尝试 复制 Gazebo 的 ros 包 功 能 。 同 样 , 它 还 在 其 操纵 器 处 理 程序 插件 上 提供 差分 驱动 功 
能 。 然 而 ,对 于 仿真 软件 中 的 传感器 .需要 使 用 V-REP Lua 脚本 创建 发 布 者 以 将 传感器 读 
数 传输 到 ROS 主题 ,而 不 能 通过 这 个 软件 包 。 

Gazebo 和 ROS 有 着 很 紧密 的 关系 。 因 为 使 用 ROS 的 人 数 众多 ,容易 从 开发 者 的 社区 
中 取得 技术 上 的 发 展 。Gazebo 有 一 个 明显 的 优势 : 虽然 它们 是 单独 的 项 目 ,但 每 一 个 版 本 
的 Gazebo 开发 ,都 有 考虑 到 相应 的 与 ROS 版 本 相 适 应 ,这 样 使 得 Gazebo 能 够 跟 紧 ROS 的 
更 新 速度 。 总 体 来 说 ,Gazebo 提供 了 一 个 广泛 的 API 来 访问 任何 代码 的 所 有 功能 。 其 次 ， 
Gazebo 已 经 集成 一 些 ROS 特定 的 功能 ,如 服务 .主题 订阅 和 发 布 。 最 后 ,更 关键 的 是 ， 
Gazebo 和 ROS 已 经 有 了 大 量 的 社区 开发 的 插件 和 代码 。 
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因此 ,虽然 V-REP 与 ROS 之 间 也 能 够 通过 Lua 或 者 插件 进行 集成 ,但 与 Gazebo 相 比 
较 , 它 们 之 间 的 即 用 型 组 件 在 数量 上 远 远 不 足 , 而 且 也 存在 较 大 的 难度 。 所 以 说 ,在 需要 与 
ROS 进程 集成 的 项 目 中 ,建议 开发 者 选择 Gazebo 这 款 仿真 软件 。 

2. 场景 建 模 

在 这 个 标准 中 ,两 个 仿真 软件 对 一 个 虚拟 的 机 器 人 工作 环境 进行 建 模 是 相对 简单 的 ,但 
也 存在 着 许多 不 同 。 这 涉及 了 可 视 化 编辑 可 视 化 场景 图 形 和 对 象 属性 等 方向 上 。 

V-REP 能 够 提供 上 述 所 有 功能 。 在 模型 浏览 器 中 ,V-REP 有 很 多 模型 ,可 以 很 容易 地 
插入 到 场景 中 。 这 些 模型 的 范围 基础 设施 对 象 ,如 墙 和 门 、 家 具 、 甚 至 地 形 模型 。 此 外 , 它 还 
有 一 个 易于 使 用 的 可 视 化 场景 图 形 。 在 那里 ,我 们 可 以 访问 场景 中 的 所 有 对 象 ,并 检查 和 修 
改 其 所 有 属性 。 

在 这 一 点 上 ,Gazebo 是 远 远 落后 的 。Gazebo 不 提供 许多 能 够 被 直接 使 用 的 场景 建 模 
功能 。 它 提供 了 一 个 建筑 编辑 器 ,这 在 设计 基本 的 环境 和 基础 设施 中 非常 实用 。 其 次 , 它 还 
提供 了 3 个 简单 的 几何 形状 ,它们 能 够 直接 插入 在 场景 中 : 一 个 球体 .一 个 立方 体 和 一 个 圆 
柱 体 。 在 这 一 点 上 ,V-REP 提供 了 更 丰富 更 多 种 类 的 基础 形状 。 最 后 ,Gazebo 拥有 一 个 由 
社区 开发 的 在 线 模型 数据 库 , 它 由 各 种 模型 组 成 ,在 Gazebo 中 可 以 直接 访问 。 这 是 Gazebo 
在 模型 构建 中 的 一 个 优点 ,但 数据 库 缺 少 官方 的 维护 ,相对 处 于 无 组 织 的 状态 。 

因此 ,在 Gazebo 中 进行 模型 的 编辑 是 不 可 能 的 。 为 此 ,用 户 必须 使 用 外 部 3D 建 模 工 
具 ( 如 Blender 或 Google Sketchup) 来 绘制 模型 ,然后 将 它们 导入 Gazebo 所 支持 的 格式 。 
Gazebo fii JH Ek Jy SDF 的 模型 格式 , 它 是 基于 XML 格式 的 一 种 文件 类 型 。 为 了 使 用 
Gazebo 的 全 部 功能 , 强烈 建议 用 户 学 习 SDF 如 何 工作 ,以 及 它 与 ROS URDF 的 关系 。 
ROS URDF 是 ROS 使 用 的 模型 格式 。 在 这 一 点 上 ,V-REP 具有 URDF 导入 工具 。 

在 这 个 标准 中 ,我 们 可 以 发 现 ,V-REP 为 场景 建 模 提 供 了 更 多 的 用 户 友 好 的 特性 ,不 需 
要 任何 深入 的 XML 知识 。 因 此 ,我 们 可 以 进行 快速 原型 设计 的 仿真 设置 和 更 复杂 的 项 目 。 
相反 ,如 果 使 用 Gazebo, 则 需要 去 深入 挖掘 SDF 的 规范 ,然后 才能 构建 那些 非 基本 的 设置 。 
最 后 ,必须 掌握 了 SDF 的 规范 后 才能 创建 非常 复杂 的 仿真 。 因 此 ,在 场景 建 模 这 一 点 上 ， 
V-REP 具有 一 定 的 优势 ,能 让 用 户 快速 地 实现 模型 构建 。 

3. 机 器 人 模型 修改 

机 器 人 模型 的 修改 与 场景 建 模 相 类 似 。 在 实际 的 项 目 中 ,在 仿真 软件 提供 的 机 器 人 模 
型 上 ,需要 向 机 器 人 模型 添加 传感器 和 执行 器 。 

要 修改 Gazebo 中 的 模型 ,用 户 需 要 使 用 SDF 文件 。 例 如 ,需要 将 接近 传感器 集成 到 
Pioneer 机 器 人 。 具 体 的 做 法 是 : 

(D 在 Gazebo 在 线 数据 库 中 使 用 Pioneer 2DX 模型 ,同时 使 用 来 自 同一 数据 库 的 
Hokuyo 激光 扫描 模型 。 虽 然 已 经 拥有 了 这 两 个 模型 :但 没有 办 法 从 Gazebo 的 内 部 将 它们 
组 合成 一 个 单一 的 模型 。 必 须 通过 编辑 适当 的 SDF 文件 来 完成 ,这 需要 花费 一 定 的 时 间 。 

© 在 感 测 部 分 ,Hokuyo 模型 带 有 一 个 ROS 插件 ,可 以 将 读数 发 布 到 ROS 主题 上 。 这 
个 插件 和 Gazebo/ROS 插件 是 类 似 的 。 
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而 在 V-REP 中 ,虽然 有 工具 来 导入 基于 XML 文件 的 模型 ,但 没有 必要 使 用 基于 XML 
的 文件 。Pioneer 2dx 的 现 有 模型 在 V-REP 本 地 模型 数据 库 上 是 默认 可 用 。 此 外 ,V-REP 
提供 各 种 不 同型 号 的 传感器 ,而且 用 户 可 以 轻松 插入 任何 现 有 型 号 的 传感器 。 传 感 器 放置 
在 机 器 人 的 某 个 部 分 中 ,所 需要 的 操作 都 是 从 VREP 内 部 完成 的 。 之 后 ,用 户 需要 修改 了 
相关 的 Lua 脚本 ,以 添加 将 传感器 的 读数 发 布 到 ROS 主题 的 ROS 发 布 者 ,这 只 需要 几 行 
代码 。 

在 这 一 点 上 ,V-REP 的 优点 是 它 的 用 户 友好 功能 。V-REP 能 够 从 仿真 软件 内 部 修改 
模型 ,这 在 具体 的 工作 上 是 非常 有 用 和 实用 的 。 唯 一 的 不 足 是 ,在 这 里 需要 使 用 的 工具 之 一 
V-Rep ROS Bridge 不 是 由 V-REP 官方 本 身 进行 维护 。 另 一 方面 ,Gazebo 不 提供 编辑 模型 
的 简单 图 形 方式 ,并 且 必 须 编辑 文本 文件 。Gazebo 具有 的 优点 是 : 大 多 数 传感器 是 由 开发 
者 社区 开发 的 解决 方案 ,如 Hokuyo 模型 。 这 使 得 用 户 很 容易 获得 很 多 非常 有 用 的 插件 ,而 
V-REP 的 机 器 人 模型 都 保存 在 自身 的 模型 浏览 器 中 ,难以 被 扩展 。 

4. 程序 控制 

在 机 器 人 仿真 软件 的 使 用 上 ,经 常 需要 用 一 段 代 码 去 控制 仿真 的 状态 。 作 为 可 用 代码 
库 的 对 仿真 环境 的 访问 和 控制 在 这 里 被 认为 是 程序 控制 的 标准 。 

在 Gazebo 和 V-REP 上 ,所 需 的 所 有 程序 控制 都 可 以 作为 与 每 个 仿真 软件 相关 联 的 
ROS 节点 提供 的 ROS 服务 。 这 项 ROS 服务 是 : 启动 和 停止 仿真 ,获取 机 器 人 模型 在 环境 
中 的 实时 姿势 ,并 设置 模型 的 姿势 。 在 Gazebo 中 , 重 置 仿真 时 ,需要 获取 和 设置 机 器 人 的 
姿势 ,这 是 因为 重 置 仿真 的 服务 出 现 问题 导致 差分 驱动 程序 插件 停止 工作 。 这 在 V-REP 
中 不 需要 ,因为 重 置 仿真 服务 也 重 置 了 机 器 人 的 位 置 。 

ROS 服务 可 以 通过 ROS 节点 代码 轻松 地 被 调用 。 所 使 用 的 数据 库 是 ROS 的 一 部 分 ， 
并 且 不 属于 任何 仿真 软件 。 这 表明 ROS 可 以 用 于 抽象 低级 实现 更 具体 的 细节 ,从 而 方便 用 
户 的 工作 。 然 而 ,Gazebo 和 VREP 都 提供 了 一 个 能 够 完全 控制 其 内 部 仿真 变量 的 代码 
API。 因 此 ,在 这 个 标准 中 ,Gazebo 和 V-REP 的 区 分 度 不 大 。 

在 这 部 分 内 容 中 ,我 们 对 V-REP 和 Gazebo 机 器 人 仿真 软件 进行 了 比较 。 得 出 的 结论 
是 : V-REP 是 一 个 更 直观 和 用 户 友好 的 仿真 软件 ,并 且 包 装 更 多 的 功能 。Gazebo 更 加 集成 
到 ROS 框架 中 ,是 一 个 开源 解决 方案 ,意味 着 它 允 许 对 仿真 软件 的 完全 控制 。 但 它 需 要 一 
些 外 部 工具 来 实现 V-REP 拥有 的 功能 。 此 外 .Gazebo E V-REP 对 于 硬件 的 要 求 更 高 。 总 
kit zi ,初学 者 需要 在 这 一 章 中 对 两 个 软件 的 学 习 进 行 比较 ,掌握 它们 在 不 同 场合 中 存在 的 
优势 ,然后 在 遇 到 具体 的 项 目 时 进行 评估 ,选择 更 适合 的 机 器 人 仿真 软件 。 


4.2 Gazebo 的 结构 


4.2.1 Gazebo 的 运行 方法 
前 面 已 经 介绍 了 Gazebo 的 安装 方法 。 接 下 来 关于 Gazebo 的 部 分 内 容 中 ,我 们 将 在 
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Ubuntu 系统 下 对 Gazebo 的 各 部 分 知识 进行 介绍 。 
在 Ubuntu 系统 环境 下 进行 以 下 的 三 个 步骤 将 在 默认 的 条 件 下 运行 Gazebo: 
确认 已 安装 Gazebo; 
© 打开 Ubuntu 的 终端 (在 大 部 分 Ubuntu 的 版 本 中 , 按 Cirl-- Alec tH f; 
@ 在 终端 的 窗口 中 通过 以 下 的 输入 命令 启动 Gazebo: 


Gazebo 


启动 Gazebo 后 ,可 以 通过 加 载 pioneer2dx( 场 景 (worlds)) 进 行 仿真 ,感受 下 Gazebo 与 
V-REP 的 不 同 。 输 入 以 下 的 命令 打开 终端 : 


Gazebo worlds/pioneer2dx. world 


1. 场景 文件 的 位 置 

运行 上 面 的 命令 ,我 们 注意 到 上 述 命令 中 的 参数 : worlds/pioneer2dx. world。 这 条 命 
令 的 含义 是 让 Gazebo 去 搜索 文件 pioneer2dx. world, 并 在 开始 仿真 的 时 候 加 载 它 。 场 景 文 
件 位 于 相对 应 版 本 的 系统 目录 中 ,例如 : Ubuntu 的 /usr/share/Gazebo-7。 如 果 在 Ubuntu7. 0 
中 安装 Gazebo ,在 终端 中 输入 下 列 命令 来 看 完整 的 场景) 目录, 输入 以 下 的 命令 : 


1s /usr/share/Gazebo - 7/worlds 
如 果 在 OS X 中 安装 Gazebo, f H] Homebrew 工具 查看 完整 的 场景 目录 ,输入 以 下 的 
命令 : 

1s /usr/local/share/Gazebo - 7/worlds 


2. 客户 端 和 服务 器 的 分 离 

在 上 面 的 例子 中 , Gazebo 命令 行 工具 实际 上 运行 了 两 个 可 执行 程序 ,一 个 是 
gzserver, 男 一 个 是 gzclient。 其 中 ,gzserver 可 执行 程序 运行 了 (自然 环境 ) 更 新 循环 和 传 
感 器 数据 计算 。gzserver 是 Gazebo 的 核心 ,可 单独 在 图 像 界 面 执 行 ,这 时 我 们 可 能 得 到 输 


出 为 “run headless”, 它 的 意思 相当 于 只 运行 gzserver。 此 外 ,gzclient 可 执行 程序 运行 在 基 
T QT 的 用 户 界面 。 这 个 应 用 程序 提供 了 一 个 优良 的 可 视 化 仿真 ,方便 让 用 户 控制 各 种 仿 
真 特性 。 


接 下 来 尝试 运行 每 一 个 可 执行 文件 ,打开 终端 并 运行 服务 器 ,输入 以 下 的 命令 : 
gzserver 
打开 另 一 个 终端 并 运行 图 像 客户 端 , 输 入 以 下 的 命令 : 


gzclient 
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此 时 ,我 们 应 该 看 到 Gazebo 的 用 户 界 面 。 我 们 可 以 重启 gzclient 应 用 程序 ,甚至 运行 
多 重 界 面 。 


4.2.2 Gazebo 的 组 成 部 分 


Gazebo 仿真 软件 的 组 成 部 分 包括 了 场景 文件 .模型 文件 .环境 变量 .Gazebo 服务 器 、 图 
像 化 客户 端 ,服务 器 及 图 像 客户 端 ,插件 等 。 下 面 对 各 部 分 进行 具体 介绍 。 

1. 场景 文件 

场景 描述 文件 包含 在 一 个 仿真 场景 中 的 所 有 元 素 ,包括 机 器 人 \ 灯光、 传感器 和 静态 对 
象 。 文 件 格式 使 用 SDF(Simulation Description Format ,并且 经 常 使 用 的 后 级 名 为 . world。 
Gazebo 的 gzserver 读 取 这 个 文件 ,并 生成 一 个 仿真 场景 。 很 多 场景 描述 文件 的 例子 是 
Gazebo 本 身 自 带 的 。 这 个 描述 文件 位 于 : < install_path >/share/ gazebo-< version >/worlds。 

2. 模型 文件 

模型 文件 同样 使 用 SDF 文件 格式 ,但 只 包含 一 个 模型 的 描述 。 这 些 文件 的 目的 是 为 了 
方便 多 次 使 用 同样 的 模型 ,并 简化 场景 文件 。 一 旦 创建 了 模型 文件 ,可 以 使 用 以 下 的 语法 将 
其 包含 在 场景 文件 中 : 


< include> 
<uri>model://model file name </uri> 
</include> 


大 量 的 模型 可 在 在 线 模型 数据 库 中 获得 。 在 Gazebo 的 一 些 旧版 本 中 有 附带 一 些 示例 
模型 。 假 设 在 网 络 连接 通畅 的 情况 下 运行 Gazebo, 我 们 可 从 数据 库 中 获取 并 插入 任意 模 
型 ,相关 内 容 将 会 在 Gazebo 运行 时 下 载 。 

3. 环境 变量 

Gazebo 使 用 大 量 环境 变量 来 定位 文件 ,建立 客户 端 和 服务 器 之 间 的 联系 。 编 译 时 ,大 
多 数 情况 用 默认 值 工作 。 这 意味 着 我 们 不 需要 设置 任何 变量 。 

下 面 是 这 些 变量 : 


GAZEBO MODEL PATH: 
冒号 紧 接 的 是 路 径 , 用 于 Gazebo 搜索 模型 。 


GAZEBO RESOURCE PATH: 


冒号 紧 接 的 是 路 径 , 用 于 搜索 其 他 资源 .例如 场景 文件 或 媒体 文件 。 


GAZEBO MASTER URI: 
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Gazebo 的 URI。 指 定 服 务 器 的 IP 和 端口 ,告诉 客户 端 连接 的 目的 地 。 


GAZEBO PLUGIN PATH: 


用 于 Gazebo 在 运行 共享 库 时 搜索 插件 。 


GAZEBO MODEL DATABASE URI: 


在 线 模型 库 的 URI, Gazebo 从 中 下 载 模型 。 
这 些 默 认 值 也 包含 在 一 个 shell 脚本 中 。 


source < install path»/share/Gazebo/setup. sh 


如 果 我 们 想 修改 Gazebo 的 环境 变量 ,例如 : 通过 它 拓展 搜索 模型 的 路 径 ,我 们 应 该 首 
先 source 上 述 的 脚本 ,然后 修改 它 。 

4. Gazebo 服务 器 

服务 器 是 Gazebo 的 主要 工作 部 分 , 它 解析 命令 行 上 给 出 的 场景 描述 文件 ,然后 使 用 物 
理 引 擎 和 传感器 来 仿真 它 。 

服务 器 可 以 用 下 面 的 命令 启动 。 注 意 , 它 在 运行 时 不 包括 任何 图 像 , 输 入 以 下 的 命令 : 


gzserver «world filename> 


< world filename > 可 以 是 : 相对 路 径 .绝对 路 径 或 环境 变量 中 的 相对 路 径 部 分 。 
Gazebo 附带 的 场景 位 于 : < 安装 路 径 >/share/Gazebo-< version number >/worlds 
例如 ,使 用 Gazebo 附带 的 empty. world, 使 用 下 面 的 命令 : 


gzserver worlds/empty. world 


5. 图 像 化 客户 端 

图 像 化 客户 端 连接 运行 中 的 gzserver 和 可 视 化 部 分 。 这 也 是 一 个 工具 ,允许 我 们 修改 
正在 运行 的 仿真 。 

运行 图 像 化 客户 端 ,输入 以 下 的 命令 : 


gzclient 


6. 服务 器 及 图 像 客户 端 
命令 Gazebo 将 服务 器 和 客户 端 同 时 执行 .而 不 是 在 运行 gzserver worlds/empty. 
world 之 后 在 运行 gzclient。 这 等 价 于 以 下 的 命令 : 


Gazebo worlds/empty. world 
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7. 插件 

插件 提供 一 个 与 Gazebo 接口 对 接 的 简便 机 制 。 插 件 可 以 在 命令 行 加 载 ,或 者 在 场景 
或 模型 文件 中 指定 (参考 SDF 格式 )。 第 一 个 加 载 在 命令 行 指 定 的 插件 ,然后 加 载 场景 或 模 
型 文件 中 指定 的 插件 。 大 多 数 插件 由 服务 器 加 载 , 然 而 插件 也 可 以 由 图 像 化 客户 端 加 载 来 
促进 GUI 计算 。 

例如 : 在 命令 行 加 载 插件 ,输入 以 下 的 命令 : 


gzserver - s «plugin filename><world file> 


图 形 化 客户 端 要 使 用 相同 的 机 制 ,输入 以 下 的 命令 : 


gzclient - g «plugin filename? 


4.2.3 Gazebo 的 结构 


1. 介绍 

与 V-REP 相 类 似 ,Gazebo 使 用 分 布 式 体系 结构 : 各 种 独立 的 库 可 用 于 物理 仿真 , 演 
染 .用 户 界 面 . 通 信和 传感器 。 此 外 ,Gazebo 提供 了 两 个 用 于 仿真 的 可 执行 程序 : 

e 一 个 服务 器 gzserver: 用 于 仿真 自然 环境 ` 泻 染 和 调用 传感器 ; 

。 一 个 客户 端 gzclient: 提供 了 一 个 可 视 化 图 形 界面 ,并 与 仿真 进行 交互 。 

此 外 ,客户 端 和 服务 器 之 间 使 用 Gazebo 的 通信 库 进行 通信 。 

2. 进程 之 间 的 通信 

目前 通信 库 使 用 开源 的 Google Protibuf 进行 消息 序列 化 和 boost::ASIO 作为 传输 机 
制 , 它 支持 发 布 或 订阅 通信 和 模式。 例如 ,虚拟 场景 中 机 器 人 姿态 的 更 新 ,传感器 的 迭代 GUI 
将 使 用 这 些 信息 来 作为 输出 。 

这 种 机 制 允许 内 部 的 仿真 ,并 提供 一 种 方便 的 机 制 来 进行 Gazebo 的 控制 。 

3. 系统 

Gazebo 的 系统 包括 了 主体 .通信 库 ,物理 库 、 泻 染 库 、 传 感 器 生成 器 .GUI 和 插件 。 

1) Gazebo 的 主体 

Gazebo 的 主体 本 质 上 是 一 个 名 为 服务 器 的 主题 。 它 提供 了 根据 名 字 搜 索 的 功能 ,主题 
管理 。 可 以 处 理 多 种 自然 环境 仿真 、 传 感 器 迭代 和 GUI。 

D 通信 库 

。 依赖 项 : Protibuf 和 boost::ASIO; 

。 外 部 API; 

。 内 部 API: 无 ; 

。 发 布 主题 : 无 ; 
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。 订阅 主题 : 无 。 

通信 库 几 乎 被 所 有 的 后 续 库 使 用 。 它 作为 Gazebo 的 通信 和 传输 机 制 , 目 前 只 支持 发 
布 或 订阅 ,但 可 以 以 最 小 代价 使 用 RPC。 

3) 物理 库 

。 依赖 项 : 动力 学 引擎 (内 部 碰撞 检测 ); 

。 外 部 API: 提供 了 一 个 用 于 物理 仿真 的 简单 而 通用 的 接口 ; 

* 内 部 API: 为 物理 库 定义 了 一 个 基本 的 接口 ,用 于 第 三 方 动力 学 引擎 。 

物理 库 提供 了 一 个 简单 而 通用 的 接口 ,用 于 基本 的 仿真 部 件 ,包括 刚体 、 磁 撞 形 状 、 关 节 
角 限 制 。 这 个 接口 集成 了 四 个 开源 物理 引擎 : Open Dynamics Engine (ODE) , Bullet, 
Simbody 和 Dynamic Animation and Robotics Toolkit. DART) 。 

用 SDF 描述 的 模型 文件 可 以 使 用 XML 加 载 这 些 物理 引擎 ,为 不 同 的 算法 实现 和 仿真 
特性 提供 了 一 种 途径 。 

4) 泻 染 库 

* 依赖 项 : OGRE; 

* 外 部 API; 

。 内 部 API。 

泻 染 库 使 用 OGRE 来 提供 一 个 为 GUI 和 传感器 库 泻 染 3D 场景 的 接口 。 这 包括 光照 、 
纹理 和 天 空 的 仿真 ,可 以 为 泻 染 引擎 引入 插件 。 

5) 传感器 生成 库 

。 依赖 项 : 泻 染 库 ,物理 库 ; 

* 外 部 API: 提供 初始 化 和 运行 的 一 系列 传感器 ; 

* 内 部 API: TBD. 

传感器 生成 库 实现 所 有 各 种 类 型 的 传感器 .通过 物理 仿真 器 监听 场景 状态 更 新 ,并 产生 
由 实例 化 传感器 指定 的 输出 。 

6) GUI 

。 依赖 项 : 泻 染 库 ,Qt; 

。 外 部 API: 无 ; 

。 内 部 API: 无 。 

GUI 库 使 用 Qt 为 用 户 创建 图 形 部 件 与 仿真 进行 交互 。 用 户 可 以 通过 GUI 窗口 小 部 
件 控制 时 间 的 流动 ,如 : 暂停 或 改变 时 间 步 长 。 用 户 也 可 以 更 改 场 景 , 通 过 添加 、 修 改 或 删 
除 模 型 。 另 外 有 一 些 工具 来 观察 日 志 记录 和 仿真 传感器 数据 。 

7) 插件 

物理 传感器 和 泻 染 库 支持 插件 。 这 些 插 件 可 以 使 用 户 访问 各 自 相 应 的 库 而 不 使 用 通信 
系统 。 
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4.3 创建 机 器 人 


4.3.1 模型 结构 和 要 求 


这 一 部 分 将 讲述 如 何 创 建 并 修改 机 器 人 模型 ,特别 是 创建 带 轮 子 的 机 器 人 。 将 机 器 人 、 
传感器 和 执行 器 进行 集成 ,最 后 通过 编程 使 模型 运动 。 

1. 内 容 提要 

Gazebo 可 以 通过 编程 方式 或 GUI 动态 地 加 载 模型 到 仿真 场景 中 。 模 型 在 下 载 后 或 由 
用 户 创造 后 存在 于 用 户 的 电脑 中 。 本 部 分 内 容 描述 Gazebo 的 模型 目录 的 结构 ,以 及 模型 
目录 必要 的 文件 。 

在 Gazebo rp ,模型 定义 为 有 着 动力 学 和 可 视 属性 的 物理 实体 模型 。 此 外 ,一 个 模型 可 
能 有 一 个 或 多 个 插件 ,这 些 插件 影响 着 模型 的 行为 。 一 个 模型 可 以 代表 任何 东西 ,从 一 个 简 
单 的 机 器 人 到 一 个 形状 复杂 的 机 器 人 ;甚至 地 面 也 是 一 个 模型 。 

Gazebo 依赖 于 一 个 数据 库 来 存储 和 维护 模型 ,以便 在 仿真 时 可 供 使 用 。 模 型 数据 库 是 
一 个 社区 支持 的 资源 ,可 以 上 传 和 维护 用 户 创造 创建 和 使 用 的 模型 。 

2. 模型 数据 存储 库 

模型 数据 存储 库 是 一 个 bitbucket 仓库 。 用 户 可 以 复制 存储 库 ,从 而 获取 模型 数据 存储 
库 中 的 模型 。 存 储 库 的 地 址 为 https://bitbucket. org/osrf/Gazebo_models, 因 此 输入 以 下 
的 命令 可 以 获取 : 


hg clone https://bitbucket. org/osrf/Gazebo models 


3. 模型 数据 库 结构 

模型 数据 库 必 须 遵 守 特 定 的 目录 和 文件 结构 规则 。 模 型 数据 库 的 根 目录 包含 着 所 有 模 
型 的 目录 和 一 个 database. config 文件 ,该 文件 包含 模型 数据 库 的 信息 。 每 一 个 模型 目录 也 
有 一 个 model. config 文件 ,其 中 包含 元 数据 模型 。 一 个 模型 目录 还 包含 模型 的 SDF 和 任何 
材料 ,网 格 信息 和 插件 。 结 构 如 下 (在 本 例 中 ,数据库 只 有 一 个 模型 model_1)。 

数据 库 如 下 : 

database. config: 元 数据 数据 库 。 目 前 这 个 是 从 CMakeLists. txt 自动 填充 ; 

model 1; model 1 目录 ; 

model. config: model 1 的 元 数据 ; 

model. sdf: 模型 的 SDF 描述 ; 

model. sdf. erb; 模型 的 Ruby HX AX SDF 描述 ; 

meshes: 所 有 COLLADA 和 STL 文件 的 目录 ; 

materials; 只 包含 textures( 纹 理 ) 和 scripts( 脚 本 ) 子 目录 的 一 个 目录 ; 

textures; 图 像 文件 的 目录 (jpg,png $); 
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scripts: OGRE 材料 脚本 的 目录 ; 

plugins; 插件 源 文件 和 头 文件 的 目录 。 

模型 目录 包含 的 子 目 录 包 含 了 插件 目录 、 网 络 目录 材料 目 录 等 ,它们 详细 的 信息 如 下 : 

CD 插件 目录 : 一 个 可 选 的 目录 ,其 中 包含 模型 的 所 有 插件 。 

(2) 网 络 目录 : 一 个 可 选 的 目录 ,其 中 包含 所 有 的 COLLADA 和 /或 STL 文件 模型 。 

(3) 材料 目录 : 一 个 可 选 的 目录 ,其 中 包含 模型 的 所 有 的 textures (纹理 ), 图 片 和 
ORGE 脚本 。 图 像 必 须 放置 在 textures( 纹 理 ) 子 目录 ,ORGE 的 脚本 文件 在 脚本 目录 中 。 

CD 数据 库 配 置 database. config 文件 在 模型 数据 库 的 根 目录 中 。 这 个 文件 包含 模型 
的 许可 信息 、 数 据 库 的 名 称 和 所 有 有 效 的 模型 的 列表 。 

注意 : database. config 文件 只 用 在 在 线 存 储 库 。 在 用 户 的 本 地 计算 机 上 完整 的 模型 的 
目录 不 需要 database. config 文件 。 

database. config 的 文件 格式 为 : 


<?xml version- '1.0'?» 

< database? 

< name > name of this database </name > 

< license» Creative Commons Attribution 3.0 Unported </license > 
< models > 

«uri» file://model_directory</uri > 

</models > 

</database > 


对 以 上 的 代码 进行 解析 ,如 下 : 

X name > 是 数据 库 的 名 称 。 在 GUI 和 其 他 工具 的 应 用 时 ,将 会 使 用 到 它 。 

<license> 是 数据 库 中 模型 的 许可 证 。Gazebo 官方 推荐 Creative Commons Attribution 3. 0 
Unported 许可 证 。 

« models > 是 数据 库 中 所 有 模型 URI 的 列表 。 

<ur> 是 模型 的 URI, 形 式 应 该 是 file; //model directory name, 

(5) 模型 配置 。 

每 个 模型 都 必须 有 一 个 model. config 模型 文件 的 根 目录 ,其 中 包含 的 元 信息 模型 。 其 
中 , model. config 文件 的 格式 是 : 


<?xml version = "1. 0"?> 

< model > 

< name > My Model Name </name > 

€ version» 1.0 </version> 

< sdf version = '1.5'» model. sdf </sdf > 
< author > 

< name > My name </name > 
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< email > name@ email. address </email > 
</author > 
<description> 

A description of the model 
</description> 
</model > 


对 以 上 的 文件 中 包含 的 代码 ,解释 如 下 : 

<name > 是 必须 项 , 它 表示 模型 的 名 称 。 

< version > 是 必须 项 , 它 该 模型 的 版 本 。 

注意 : 这 不 是 模型 使 用 SDF 的 版 本 。 模 型 使 用 SDF 信息 是 保存 在 model. sdf 文件 。 

X sdf > 是 必需 项 , 它 描 述 这 个 模型 的 SDF 或 URDF 文件 的 名 称 。version 属性 显示 的 
文件 使 用 SDF 版 本 ,而 使 用 URDFs 时 不 是 必需 的 。 为 了 支持 多 个 sdf 版 本 ,可 以 使 用 多 个 
< sdf > 元 素 。 

X author > 是 必需 项 , 它 表 示 作 者 。 

<name> 是 必需 项 , 它 表示 模型 作者 的 名 称 。 

* «email > 是 必需 项 , 它 表 示 作 者 的 电子 邮件 地 址 。 

< description > 是 必需 项 , 它 表示 模型 的 描述 ,其 中 模型 的 描述 应 该 包括 : 模型 的 种 类 
(如 机 器 人 ,桌子 ,杯子 等 模型 ); 插件 的 用 途 ( 即 模型 的 功能 ) 。 

< depend > 是 可 选 选项 。 它 表示 这 个 模型 的 所 有 依赖 项 ,这 通常 是 其 他 模型 。 

< model > 是 可 选 选项 。 

<uri> 是 必需 项 , 它 表示 URI 模式 的 依赖 。 

€ version > 是 必需 项 , 它 表 示 版 本 的 模型 。 

(6) 模型 SDF 描述 格式 。 

每 个 模型 需要 一 个 model. sdf 文件 .这 其 中 包含 模型 的 仿真 器 描述 格式 。 用 户 可 以 在 
SDF 的 网 站 (http://sdformat. org/) 找 到 更 多 的 信息 。 

(7) 模型 SDF. ERB. 

标准 的 文件 SDF 可 以 包含 Ruby 代码 。 这 个 选项 用 于 通过 编程 方式 嵌入 Ruby 代码 模 
板 去 生成 SDF 文件 。 请 注意 ,Ruby 转换 应 该 手动 完成 ,最 后 model. sdf 文件 必须 和 模型 
sdf. erb 文件 一 起 上 传 。 

此 外 ,可 以 在 gazebo_models 存储 库 中 找到 sdf. erb 文件 的 示例 (其 中 的 一 些 使 用 弃 用 
的 后 缀 . rsdf )。 一 个 简单 的 ERB 文件 是 flocking. world. erb, 它 使 用 一 个 简单 的 循环 。 


4.3.2 模型 的 上 传 


Gazebo 与 V-REP 的 不 同 点 之 一 在 于 模型 能 够 共享 。 本 部 分 将 介绍 如 何 上 传 自 己 制 作 
的 模型 到 社区 中 。 假 设 用 户 在 Bitbucket 有 一 个 账户 ,并 且 有 一 个 Mercurial 的 客户 端 。 
复制 osrf/Gazebo models 存储 库 转 到 https: // bitbucket. org/osrf/Gazebo models. fA 
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后 从 屏幕 左 侧 的 菜单 中 选择 Fork” ,并 选择 默认 选项 即 可 。 在 用 户 成 功 分 又 存储 库 之 
后 ,复制 它 。 假 定 用 户 为 存储 库 选择 了 默认 名 称 , 则 将 使 用 类 似 于 以 下 命令 的 命令 进行 
复制 : 


code $ hg clone https://yourname(@bitbucket. org/yourname/Gazebo models 


其 中 ,yourname 是 用 户 的 Bitbucket HP! 4. 

1. 创建 模型 

在 Gazebo_models 目录 下 创建 在 用 户 的 模型 目录 。 对 于 本 部 分 内 容 , 我 们 假定 这 个 目 
录 称 为 mymodel。 该 目录 必须 包括 文件 model. config, 并 且 它 也 可 以 包括 其 他 文件 (插件 ， 
makefile, README 等 ) , 

TA model. config 文件 提供 必要 信息 以 挑选 合适 的 SDF 文件 ,模型 的 作者 的 信息 ,该 模 
型 的 文字 描述 。 

示例 model. config 如 下 所 示 : 


<?xml version = "1. 0"?> 

< model > 

< name > Wedge juggler </name > 

€ version» 1.0 </version> 

< sdf version = "1.5"» model. sdf </sdf > 
< author > 

< name > Evan Drumwright </name > 
< email > drum@gwu. edu </email > 
</author > 

<description> 

A ball - in- wedge juggler. 
</description> 

</model > 


这 个 model. config 文件 位 于 model. sdf 中 ,并 遵循 SDF 标准 1.5, 它 指示 仿真 器 的 模型 
定义 ( 即 视觉 ,惯性 ,运动 和 几何 属性 等 )。 此 外 ,我 们 可 以 定义 模型 的 多 个 版 本 ,这 可 以 用 于 
用 户 想 将 模型 与 不 同 版 本 的 Gazebo 一 起 使 用 的 场合 。 例如, 我们 现在 改变 上 面 的 文件 的 
内 容 , 支 持 三 个 不 同 版 本 的 SDF: 


<?xml version = "1. 0"?> 

< model > 

< name > Wedge juggler </name > 

€ version» 1.0 </version> 

< sdf version = "1. 5"> model. sdf </sdf > 

< sdf version = "1. 4"> model - 1. 4. sdf </sdf > 
< author > 
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< nane > Evan Drumwright </name > 
< email > drum@gwu. edu </email > 
</author > 

<description> 

A ball - in- wedge juggler. 
</description> 

</model > 


2. 将 目录 (和 文件 ) 添 加 到 存储 库 
用 户 可 以 通过 键入 以 下 内 容 将 所 有 文件 添加 到 存储 库 ,输入 以 下 的 命令 : 


Gazebo models $ hg add mymodel 


或 者 ,如 果 用 户 有 一 些 不 想 被 发 现 的 文件 ,可 以 单独 添加 文件 ,输入 以 下 的 命令 : 


Gazebo models $ hg add mymodel/model. config 
Gazebo models $ hg add mymodel/model. sdf 


3. 提交 并 推送 用 户 的 更 改 到 Bitbucket. 输 入 以 下 命令 


Gazebo models $ hg commit 
Gazebo_models $ hg push 


4. 最 后 一 步 : 创建 合并 分 支 请 求 

从 到 bitbucket Æ https:;//bitbucket. org/yourname/Gazebo _ models (假设 用 户 
bitbucket 的 用 户 名 是 yourname 并 且 使 用 了 默认 的 分 叉 库 ), 创 建 一 个 合并 分 支 请 求 。 从 网 
页 左 侧 的 菜单 中 选择 “创建 请 求 "。 确 保 “osrf/Gazebo_models” 被 选择 在 箭头 的 右边 。 如 果 
用 户 对 其 他 选项 感到 满意 ,请 单 击 “ 创 建 请 求 "按钮 。OSRF 将 审查 用 户 的 合并 分 支 请 求 ,并 
开始 将 用 户 的 更 改 集成 到 模型 数据 库 中 。 


4.3.3 制作 一 个 模型 


1. 内 容 提要 

本 部 分 内 容 描述 了 SDF 模型 对 象 的 详细 信息 。SDF 模型 可 以 从 简单 形状 到 复杂 机 器 
人 。 它 指 的 是 < model > SDF 标签 .本质 上 是 连 杆 .关节 、 磁 撞 对 象 .视觉 和 插件 的 集合 。 根 
据 所 需 模型 的 复杂 性 ,生成 模型 文件 可 能 很 困难 。 此 页 面 将 提供 有 关 如 何 构建 模型 的 一 些 
注意 。 

2. SDF 模型 的 组 件 

Links; 连接 包含 模型 实体 的 物理 属性 。 这 可 以 是 车 轮 ,或 者 是 连接 链 中 的 连 杆 。 每 个 
连 杆 可 以 包含 许多 碰撞 和 视觉 元 素 。 尝 试 减少 模型 中 的 连 杆 数 , 以 提高 性 能 和 稳定 性 。 例 
如 ,桌子 模型 可 以 包括 通过 关节 连接 的 5 个 连 杆 (4 个 用 于 腿 部 ,1 个 用 于 顶部 )。 然 而 ,这 过 
于 复杂 ,特别 是 因为 关节 永远 不 会 移动 。 所 以 .应 以 1 个 连 杆 和 5 个 磁 撞 体 组 成 桌子 的 
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模型 。 

Collision; Collision 元 素 封 装 用 于 碰撞 检查 的 几何 体 。 这 可 以 是 简单 的 形状 (这 是 首 
选 ) 或 三 角形 网 格 (消耗 更 多 的 资源 )。 连 杆 可 能 包含 许多 Collision 元 素 。 

Visual: Visual 元 素 用 于 连 杆 的 可 视 化 部 分 。 连 杆 可 以 包含 多 个 Visual 元 素 。 

Inertial; Inertial 元 素描 述 链 路 的 动态 特性 .如 质量 和 惯性 旋转 矩阵 。 

Sensor; Sensor 收集 来 自 世界 的 数据 以 供 插 件 运 用 。 连 杆 可 能 包含 0 个 或 更 多 的 传 
感 器 。 
Joints: — Joints 连接 两 个 连 杆 。 根 据 其 他 参数 建立 父子 关系 ,例如 旋转 轴 和 关节 
限制 。 

Plugins; 插件 是 由 第 三 方 创建 的 共享 库 , 用 于 控制 模型 。 

3. 构建 模型 

步骤 1: 收集 网 格 。 

这 个 步骤 涉及 收集 构建 模型 所 需 的 所 有 必需 的 3D 网 格 文件 。Gazebo 提供 了 一 组 简单 
的 形状 : box sphere 和 cylinder。 网 格 来 自 多 个 地 方 。Google's 3D warehouse 是 一 个 很 好 
的 3D 模型 库 。 最 后 ,用 户 可 以 使 用 3D 建 模 器 (如 Blender sk Sketchup) 制 作用 户 自己 的 网 
格物 体 。Gazebo 需要 将 网 格 文件 的 格式 为 STL 或 Collada, 其 中 Collada 是 首选 格式 。 此 
外 ,有 以 下 三 点 需要 注意 的 地 方 : 

。 使 用 3D 建 模 软 件 移动 每 个 网 格 ,使 其 中 心 为 原点 。 这 将 使 模型 在 Gazebo 中 的 放置 


更 容易 。 
* Collada 文件 格式 允许 用 户 将 材料 附加 到 网 格 。 使 用 此 机 制 来 改善 网 格 的 视觉 
外 观 。 


。 保持 网 格 简单 。 如 果 用 户 打算 使 用 网 格 作为 碰撞 元 素 , 这 是 尤其 真实 的 。 一 个 常见 
的 做 法 是 使 用 低 多 边 形 网 格 作为 collision 元 素 ,高 层 多 边 形 网 格 用 于 视觉 。 更 好 的 
做 法 是 使 用 内 置 的 形状 (box,sphere,cylinder) 作 为 collision 元 素 。 
步骤 2: 制作 用 户 的 模型 SDF 文件 。 
首先 创建 一 个 非常 简单 的 模型 文件 .或 复制 一 个 现 有 的 模型 文件 。 这 里 以 一 个 简单 的 
盒子 模型 作为 介绍 。 它 是 只 有 一 个 单位 大 小 的 箱子 ,并 且 可 以 用 于 碰撞 , 且 具 有 单位 惯性 。 
首先 我 们 需要 创建 box. sdf 模型 文件 ,输入 以 下 的 命令 


gedit box. sdf 


将 以 下 内 容 复制 到 box. sdf 中 : 


<?xml version- '1.0'?> 

< sdf version = "1. 4"> 

< model name = "my model" 
<pose>0 0 0.5 0 0 0</pose> 
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< static» true </static > 
< link name = "link" 


< inertial > 

< mass > 1. 0 </mass> 

< inertia><! —— interias are tricky to compute 一 一 > 

<! — http://answers. Gazebosin. org/question/4372/the - inertia - matrix- explained/ --> 
< ixx> 0.083 </ixx><! —— fora box: ixx = 0.083 * mass * (y*y + z*z) 一 -> 

< ixy>0.0</ixy><! — for a box: ixy = 0 -一 > 

Xixz»0.0«/ixz»«! — forabox: ixz » 0 —» 


< iyy> 0.083 </iyy><! -- fora box: iyy 
«iyz»0.0«/iyz»«! — fora box: iyz = 
< izz>0.083 </izz><! -- fora box: izz 
</inertia> 

</inertial > 

<collision name = "collision"> 

< geometry > 

Xbox? 

<size>1 1 1</size> 

</box> 

</geometry > 

</collision> 

< visual name = "visual"> 

< geometry > 

<box> 

<size>1 1 1 </size> 

</box> 

</geometry > 

</visual > 

</link> 

</model > 

</sdf > 


0.083 * mass * (x*x + z*z) --> 
一 > 


"o"o o t 


0.083 * mass * (x*x + yxy) --> 


注意 : 

(1) 箱 体 几何 的 原点 在 箱 体 的 几何 中 心 , 因 此 为 了 使 箱 体 的 底部 与 地 平面 齐 平 ,指令 
<pose>000.5000</pose> 的 作用 是 将 箱 体 的 原点 提升 到 地 面 上 。 

(2) 上 述 示例 将 箱子 模型 简单 地 设置 为 静态 ,这 使 得 模型 不 可 移动 。 此 功能 在 模型 创 
建 期 间 很 有 用 。 创 建 模型 后 ,< static > 如 果 希 望 模型 可 移动 ,请 将 标记 设置 为 false。 

步骤 3: 添加 到 模型 SDF 文件 。 

接 下 来 是 通过 对 . sdf 文件 的 编写 ,逐步 增加 的 添加 项 增加 了 文件 的 复杂 性 。 每 次 新 添 
加 ,使 用 图 形 客 户 端 加 载 模型 ,以 确保 用 户 的 模型 是 正确 的 。 

以 下 是 添加 功能 的 参考 顺序 : 


第 4 章 ”Gazebo 在 机 器 人 仿真 中 的 应 用 | 381 


CD 添加 连 杆 。 

@ 设置 碰撞 元 素 。 

O 设置 可 视 元 素 。 

@ 设置 惯性 属性 。 

C 转 到 步骤 ,直到 所 有 连 杆 都 已 添加 。 
© 添加 所 有 关节 (如 果 有 )。 

© 添加 所 有 插件 (如 果 有 ) 。 


4.3.4 ”制作 移动 机 器 人 模型 


1. 内 容 提要 

本 部 分 内 容 演 示 了 Gazebo 的 基本 模型 管理 ,用 户 完成 使 用 差分 驱动 机 制 创建 移动 的 
双 轮 移动 机 器 人 的 过 程 ,以 便 熟悉 模型 数据 库 中 的 基本 模型 表示 。 

2. 创建 模型 目录 

阅读 模型 数据 库 文档 后 ,用户 将 创建 自己 的 模型 ,因为 Gazebo 模型 数据 库 目录 结构 必 
须 遵循 格式 化 规则 。 

CD 创建 模型 目录 ,输入 以 下 命令 : 


mkdir -p —/.Gazebo/models/ny robot 


(2) 创建 模型 配置 文件 输入 以 下 命令 : 


gedit ~/.Gazebo/models/my_robot/model. config 


G) 复制 粘贴 以 下 内 容 : 


<?xml version = "1. 0"?> 

< model > 

< name > My Robot </name > 
<version> 1.0 </version> 

< sdf version = '1. 4'> model. sdf </sdf > 
< author > 

< name > My Name </name > 

< email > me@ny. email </email > 
</author > 

< description» 

My awesome robot. 
</description> 

</model> 


(4) 创建 一 /. Gazebo/models/my_robot/model. sdf 文件 ,输入 以 下 的 命令 : 


gedit ~/.Gazebo/models/my_robot/model. sdf 
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(5) 复制 粘贴 以 下 的 内 容 : 


<?xml version- '1.0'?> 

< sdf version = '1.4'> 

< model name = "my_robot"> 
</model> 

</sdf > 


到 此 时 ,我 们 有 一 个 模型 的 基本 内 容 。 该 model. config 文件 描述 了 一 些 额外 的 元 数据 
的 机 器 人 。 该 model. sdf 文件 包含 Gazebo 连 杆 实例 化 的 机 器 人 所 需要 的 必要 的 标签 ,该 实 
例 化 的 机 器 人 为 SDF 1.4 版 本 的 ,其 名 字 为 my_robot。 

3. 构建 模型 的 结构 

此 步骤 将 创建 一 个 带 有 两 个 轮子 的 矩形 基 座 。 第 一 步 是 布局 模型 的 基本 形状 。 要 做 到 
这 一 点 ,将 使 我 们 的 模型 static, 这 意味 着 它 将 被 物理 引擎 忽略 。 因 此 ,模型 将 留 在 一 个 地 
方 , 并 允许 正确 对 齐 所 有 的 组 件 。 

(1) 通过 添加 文件 一 /. Gazebo/models/my_robot/model. sdf 中 的 部 分 < static > true 
</static > 来 使 模型 静止 。 这 里 ,我 们 给 出 的 例子 为 : 


<?xml version = '1.0'?> 

< sdf version = '1.4'> 

< model name = "my_robot"> 
< static» true </static > 
«/nodel > 

</sdf > 


(2) 通过 编辑 一 /. Gazebo/models/my_robot/model. sdf 文件 来 添加 矩形 基 座 。 这 里 
给 出 的 例子 为 : 


<?xml version = '1.0'?» 

< sdf version = '1.4'» 

< model name = "my robot" 
«static» true </static > 

< link name = 'chassis'^ 
<pose>0 0.1 0 0 0</pose> 
€ collision name = 'collision'> 
< geometry > 

< box > 

<size>.4 .2 .1</size> 
</box> 

</geometry > 

</collision> 

< visual name = 'visual'> 
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<geometry> 

<box> 

<size>.4 .2 .1</size> 
</box> 

</geometry> 

</visual > 

</link> 

</model> 

</sdf > 


创建 了 一 个 0.4X0.2X0.1 米 大 小 的 box, collision 部 分 可 以 指定 由 所 述 的 碰撞 检测 
引擎 中 使 用 的 形状 。visual 元 件 可 以 指定 由 泻 染 引 擎 使 用 的 形状 。 对 于 大 多 数 使 用 情况 ， 
collision 和 visual 元 素 是 相同 的 。 使 用 不 同 的 collision 和 visual 元 素 的 最 常见 的 用 法 是 将 
visual 使 用 复杂 网 格 的 元 素 简化 后 作为 collision 元 素 。 这 将 有 助 于 提高 性 能 。 

(3) 通过 运行 Gazebo, 并 通过 GUI 上 的 插入 模型 界面 导入 模型 来 试用 用 户 的 模型 。 如 
图 4-1 所 示 ,用 户 应 该 看 到 一 个 白色 的 盒子 漂浮 在 地 面 以 上 1 米 。 


图 4-1 导入 的 白色 盒子 


(4) 现在 给 机 器 人 添加 一 个 脚轮 。 脚 轮 是 一 个 没有 摩擦 的 球体 。 这 种 脚轮 比 添加 带 有 
关节 的 轮子 更 好 ,因为 它 对 物理 引擎 设置 了 较 少 的 约束 ,输入 以 下 的 内 容 : 


<?xml version = '1.0'?» 

< sdf version = '1.4'> 
«model name = "my_robot"> 
< static> true </static > 

< link name = 'chassis'> 
<pose> 0 0 .1 0 0 0 </pose> 
<collision name = 'collision'> 
< geometry > 

Xbox? 

<size> .4 .2 .1</size> 
</box> 

</geometry> 

</collision> 
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以 上 的 内 容 能 够 显示 用 户 的 模型 ,以 确保 脚轮 出 现在 机 器 人 的 末端 。 如 图 4-2 所 示 ,在 
Gazebo 中 将 会 生成 相应 模型 (用 户 不 需要 重新 启动 Gazebo, 它 会 每 次 修改 它 后 从 磁盘 重新 
加 载 用 户 的 修改 模型 ) 。 

(5) 现在 让 我 们 添加 一 个 左轮 。 将 一 /. Gazebo/models/my_robot/model. sdf 文件 修 
改 为 以 下 内 容 : 


第 4 章 


Gazebo 在 机 器 人 仿真 中 的 应 用 | 385 


图 4-2 添加 了 脚轮 的 模型 


<?xml version = '1.0'?> 
< sdf version = '1.4'» 
< model name = "my_robot"> 
< static > true </static > 
< link name = 'chassis'> 
«pose» 00.1 00 0 </pose> 
<collision name = 'collision'> 
< geometry > 
<box> 
<size> .4 .2 .1 </size> 
</box> 
</geometry > 
</collision> 
< visual name = 'visual'> 
< geometry > 
<box> 
<size> .4 .2 .1 </size> 
</box> 
</geometry> 
</visual > 
< collision name = 'caster collision'> 
«pose» -0.150 -0.05000</pose> 
< geometry > 
< sphere? 
< radius? .05 </radius> 
</sphere > 
</geometry > 
< surface> 
< friction> 
<ode> 

o° 

" 
< slipl > 1.0 </slipl> 
< slip2> 1.0 </slip2 > 
</ode> 
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</friction> 

</surface> 

</collision> 

< visual name = 'caster_visual'> 
«pose» -0.15 0 -0.05 0 0 0 </pose> 
< geometry > 

< sphere > 

< radius> . 05 </radius> 

</ sphere > 

</geometry > 

</visual > 

</link> 

< link name = "left_wheel"> 
<pose>0.1 0.13 0.1 0 1.5707 1.5707 </pose> 
X collision name = "collision"> 
< geometry > 

<cylinder> 

< radius >. 1 </radius > 

< length>. 05 </length > 
</cylinder > 

«/geonetry > 

</collision> 

< visual name = "visual"> 

< geometry > 

< cylinder > 

< radius >. 1 </radius > 

< length>. 05 </length> 
</cylinder > 

«/geonetry > 

</visual > 

</link> 

</model > 

</sdf > 


之 后 运行 Gazebo, 如 图 4-3 所 示 ,插入 用 户 的 机 器 人 模型 ,并 确保 车 轮 已 出 现在 正确 的 位 置 。 


AT 


图 4-3 添加 了 左轮 的 模型 
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(6) 我 们 可 以 通过 复制 左轮 ,并 调整 轮 连 杆 的 姿势 来 制作 右 轮 输入 以 下 内 容 : 
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如 图 4-4 所 示 ,此 时 机 器 人 应 该 有 一 个 带 有 脚轮 和 两 个 轮子 的 底盘 。 


第 4 章 ”Gazebo 在 机 器 人 仿真 中 的 应 用 | 其 389 


图 4-4 带 有 脚轮 和 两 轮子 的 模型 


CT) 通过 设置 < static >% false ,使 模型 呈 动 态 ,为 左右 轮 添加 两 个 铵 连 杆 头 。 


<?xml version = '1. 0'?> 

< sdf version ='1.4'> 

< model name = "my robot" 

< static» false </static > 

< link name = 'chassis'> 
«pose» 00.1000 </pose> 
<collision name = 'collision'» 
< geometry > 

Xbox? 

<size> .4 .2 .1 </size> 
</box> 

</geometry> 

</collision> 

< visual name = 'visual'> 

< geometry > 

< box > 

<size> .4 .2 .1 </size> 
</box> 

«/geonetry > 

</visual > 

<collision name = 'caster collision'> 
«pose» -0.150 -0.05000</pose> 
< geometry > 

< sphere> 

«radius? .05 </radius > 
</sphere> 

</geometry> 

«surface? 

«friction» 

«ode» 
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< visual name = "visual"> 
<geometry> 

<cylinder> 

<radius> .1 </radius> 

< length» .05 </length> 
</cylinder > 

</geometry > 

</visual > 

</link> 

< joint type = "revolute" name= "left_wheel_hinge"> 
<pose>0 0 —0.03 0 0 0</pose> 
<child> left wheel </child > 

< parent > chassis </parent > 
<axis> 

<xyz>0 1 0</xyz> 

</axis> 

</joint> 

< joint type = "revolute" name = "right wheel hinge"> 
< pose» 0 0 0.03 0 0 0 </pose> 
«child» right wheel </child> 

< parent > chassis </parent > 
«axis? 

<xyz>010</xyz> 

</axis> 

</joint> 

</model> 

</sdf > 


其 中 ,代码 “<xyz> 0 10</xyz>” 的 意思 是 两 个 关节 绕 Y 轴 旋 转 ,并 将 每 个 轮 连接 到 机 箱 。 

(8) 启动 Gazebo, 然 后 插 和 人 用户 的 模型 。 单 击 屏幕 右 侧 的 点 ,并 将 它们 向 左 拖 动 。 

(9) 将 出 现 一 个 新 窗口 .其 中 包含 每 个 关节 的 各 种 控制 器 (注意 : 确保 选择 要 控制 的 型 号 ) 。 

(10) 在 Force 标签 页 下 ,将 施加 到 每 个 接头 的 力 增加 到 约 0.1N。m。 如 图 4-5 Bron 
此 时 机 器 人 应 该 四 处 移动 。 


4-5 可 移动 的 机 器 人 
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4.3.5 导 人 网 格 


本 部 分 内 容 介 绍 如 何 将 3D 网 格 导 入 Gazebo, 

1. 准备 网 格 

Gazebo 使 用 右手 坐标 系 ,其 中 十 Z 方 向 向 上 (垂直 ) ,十 X 方向 向 前 (进入 屏幕 ) ,十 Y 方 
向 向 左 。 

1) 降低 复杂 性 

与 V-REP 相似 ,许多 网 格 可 能 过 于 复杂 。 为 了 效率 ,具有 数 千 个 三 角形 的 网 格 应 该 被 
缩小 或 分 割 成 单独 的 网 格 。 

2) 居中 网 格 

设置 居中 网 络 ,第 一 步 是 将 网 格 居中 在 (0,0,0) ,并 沿 着 X 轴 定 向 前 面 (可 以 是 主观 设 
定 的 ) 。 

3) 缩放 网 格 

Gazebo 使 用 公制 系统 。 许 多 网 格 (尤其 是 来 自 3D 仓库 的 网 格 ) 使 用 英制 单位 。 使 用 用 
户 的 3D 编辑 器 将 网 格 缩放 到 公制 尺寸 。 

2. 导出 网 格 

一 旦 网 格 已 经 准备 好 ,将 其 导出 为 Collada 文件 。 此 格式 将 包含 所 有 3D 信息 和 材料 。 

3. 测试 网 格 

测试 网 格 的 最 简单 的 方法 是 创建 一 个 简单 的 场景 文件 my. mesh. world 去 加 载 网 格 。 
用 以 下 的 内 容 去 替换 文件 名 为 my_mesh. dae 的 网 格 。 


<?xml version = "1. 0"?> 

< sdf version = "1. 4"> 

< world name = "default"> 

< include> 

<uri> model://ground plane </uri> 
</include> 

<include> 

X uri? model://sun</uri> 

«/ include» 

< model nane = "ny mesh" 

«pose» 0000 0 0</pose> 

< static» true </static > 

< link name = "body"> 

< visual name = "visual" 

< geometry > 

<mesh><uri> file://my nesh. dae «/uri»«/nesh» 
X/geonetry > 

</visual > 

</link> 
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«/nodel > 
</world> 
</sdf > 


然后 只 需 在 文件 所 在 的 目录 中 启动 Gazebo: 
Gazebo my mesh. world 


4. 测试 网 格 

用 户 可 以 使 用 文件 名 为 duck. dae 和 duck. png 的 网 格 文件 。 将 它们 放 在 与 场景 文件 相 
同 的 目录 中 。 因 为 鸭子 的 模型 被 定义 为 Y 轴 为 向 上 ,用 户 可 以 在 sdf 中 旋转 ,使 其 显示 垂 
直 , 设 置 的 内 容 如 下 : 


< visual name = "visual" 
<pose>0001.570800</pose> 

< geometry > 

< mesh>< uri > file://duck. dae «/uri ></mesh> 
</geometry > 

</visual > 


生成 的 场景 模型 如 图 4-6 所 示 。 


图 4-6 网 格 文件 生成 的 鸭子 模型 


4.3.6 附加 网 格物 体 


1. 内 容 提 要 

网 格 最 常见 的 作用 是 创建 逼真 的 视觉 效果 , 它 可 以 为 模型 的 视觉 和 传感器 添加 真实 感 。 
本 部 分 内 容 演示 了 用 户 如 何 使 用 自 定义 网 格 来 定义 模型 在 仿真 中 的 显示 方式 。 

2. 将 网 格 连 接 为 视觉 

COD 切换 到 my robot 目录 ,输入 以 下 命令 : 


cd ~/.Gazebo/models/my_robot 
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(2) 使 用 用 户 的 编辑 器 打开 model. sdf 文件 .输入 以 下 命令 : 


gedit 一 /. Gazebo/models/my_robot/model. sdf 


CD 我 们 将 在 箱子 中 添加 一 个 网 格 。 找 到 视觉 效果 name 一 visual, 格 式 如 下 : 


< visual name = 'visual' 
< geometry > 

<box> 

<size>.4 .2 .1</size> 
</box> 

</geometry > 

</visual > 


(4) 网 格 可 以 来 自 磁盘 上 的 文件 ,或 来 自 另 一 个 模型 。 在 这 个 例子 中 ,将 使 用 
pioneer2dx 模型 中 的 网 格物 体 。 保 持 文件 的 其 余部 分 不 变 , 将 可 视 元 素 更 改 为 以 下 内 容 : 


< visual name = 'visual'> 

< geometry > 

<mesh> 

<uri> model://pioneer2dx/meshes/chassis. dae </uri> 
</mesh> 

</geometry> 

</visual > 


(5) 查看 用 户 的 本 地 缓存 模型 数据 库 ,看 看 用 户 是 否 有 pioneer2dx 上 面 的 < mesh > 块 
引用 的 模型 ,输入 以 下 的 命令 : 


ls —1 一 /.Gazebo/models/pioneer2dx/meshes/chassis. dae 


如 果 网 格 文件 不 存在 ,通过 在 Gazebo 中 至 少 产生 一 次 Pioneer 2DX 模型 ,使 可 以 
Gazebo 从 模型 数据 库 中 提取 模型 。 或 者 用 户 可 以 手动 将 模型 文件 下 载 到 本 地 缓存 ,输入 以 
下 的 命令 : 


cd ~/.Gazebo/models 
wget -q -R * index. htmlx , * .tar.gz -- no- parent -r -x - nH http://models. Gazebosim. 
org/pioneer2dx/ 


(6) 在 Gazebo 中 , 拖 动 My Robot 场景 中 的 模型 。 如 图 4-7 所 示 , 机 箱 的 外 观看 起 来 像 
一 个 pioneer2dx。 

(7) 显然 ,机 箱 对 于 机 器 人 来 说 太 大 了 ,所 以 需要 缩放 视觉 标签 。 利 用 以 下 的 内 容 , 修 
改 visual 标签 以 具有 缩放 因子 。 
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图 4-7 场景 中 的 机 箱 模型 


< visual name = 'visual'> 

< geometry > 

<mesh> 

<uri> model: //pioneer2dx/meshes/chassis. dae </uri > 
<scale>0.9 0.5 0.5 </scale> 


</mesh> 
</geometry > 
</visual > 


(8) 在 Z 轴 上 ,机 器 人 的 外 观 视 觉 较 低 。 输 入 以 下 的 内 容 , 可 以 通过 指定 视觉 的 姿态 来 
提高 一 点 : 


<visual name = 'visual'» 
<pose>000.05000</pose> 


< geometry > 

<mesh> 

<uri> model: //pioneer2dx/meshes/chassis. dae </uri> 
<scale> 0.9 0.5 0.5 </scale> 

</mesh> 

«/geonetry > 

</visual > 


注意 ,在 这 一 点 上 ,我们 只 是 简单 地 修改 了 机 器 人 的 < visual > 元 素 , 所 以 可 以 通过 GUI 
和 基于 GPU 的 传感器 (如 相机 、 深 度 相机 和 GPU 激光 器 ) 使 机 器 人 看 起 来 像 pioneer2DX 
模型 的 缩小 版 本 。 因 为 没有 修改 < collision > 这 个 模型 中 的 元 素 , 所 以 其 将 仍然 被 物理 引擎 
的 动态 碰撞 ,并 且 GPU 的 射线 传感器 是 盒子 形状 的 。 
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4.3.7 给 机 器 人 添加 传感器 


1. 内 容 提要 

本 部 分 内 容 演示 了 用 户 如 何 通 过 使 用 < include > 标签 和 < joint > 来 连接 复合 模型 中 不 
同 组 件 ,直接 从 Gazebo 模 型 数据 库 中 模型 创建 复合 模型 。 

2. 添加 镭射 器 

向 机 器 人 或 任何 模型 添加 激光 仅仅 是 将 传感器 包括 在 模型 中 。 

CD 进入 用 户 的 上 一 个 部 分 内 容 的 模型 目录 ,输入 以 下 的 命令 : 


cd —/.Gazebo/nmodels/my robot 


(2) 在 编辑 器 中 打开 model. sdf, 
(3) 在 文件 末尾 附近 的 </model > 标记 之 前 直接 添加 以 下 内 容 : 


< include> 

<uri> model://hokuyo </uri > 
<pose>0.2 0 0.2 0 0 0</pose> 
</include> 

< joint name = "hokuyo_joint" type= "revolute"> 
< child» hokuyo: : link </child > 
< parent > chassis </parent > 
«axis? 

<xyz>0 0 1 </xyz > 

<limit> 

< upper > 0 </upper > 

< lower > 0 </lower > 

</limit> 

</axis> 

</joint> 


上 述 内 容 的 作用 分 别 是 : < include > 块 告诉 Gazebo 需要 找到 一 个 模型 ,并 根据 给 定 的 
< pose> 值 插入 到 相关 的 父 模型 中 。 在 这 种 情况 下 ,我们 将 hokuyo 激光 器 放置 在 机 器 人 的 
前 方 和 上 方 。< uri > 块 告诉 Gazebo 在 其 模型 数据 库 中 找到 模型 (注意 ,用 户 可 以 在 此 处 或 
在 相应 存储 库 查 看 该 部 分 内 容 中 用 到 的 数据 库 列 表 uri) 。 新 的 <joint > 将 插入 的 hokuyo 激 
光 器 连接 到 机 器 人 的 机 箱 。 接 头 具 有 与 < upper > 和 < lower > 零 限 制 , 以 防止 其 移动 。 

< child > 的 名 称 是 从 hokuyo 模型 的 SDF 派生 . 它 开头 部 分 的 内 容 为 : 


<?xml version = "1.0"?> 
< sdf version ="1.4"> 
< model name = "hokuyo"> 
< link name = "link"> 
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当 hokuyo 模型 被 插入 时 ,进行 hokuyo 的 连 杆 以 其 模型 名 称 命名 。 在 这 种 情况 下 , 模 
型 名 称 为 hokuyo, 因 此 hokuyo 模型 中 的 每 个 连 杆 都 以 hokuyo 开头 。 

(4) 现在 启动 Gazebo, 并 使 用 GUI 上 的 “插入 "选项 卡 将 机 器 人 添加 到 仿真 中 。 如 图 4-8 
所 示 ,用 户 应 该 看 到 带 激光 的 机 器 人 。 


图 4-8 带 激光 的 机 器 人 


(5) 尝试 向 机 器 人 添加 摄像 头 。 相 机 的 模型 URI 是 model://camera', 它 应 该 是 本 地 组 
存 为 用 户 ,输入 以 下 的 命令 : 


1s ~/.Gazebo/models/camera/ 


4.3.8 做 一 个 简单 的 夹 持 器 


下 面 介绍 如 何 通 过 编辑 SDF 文件 做 一 个 简单 的 两 指 夹 持 器 。 
1. 设置 用 户 的 模型 目录 

本 部 分 内 容 参考 模型 数据 库 文件 和 SDF 文件 。 

2. 制作 模型 

COD 创建 场景 文件 的 目录 ,输入 以 下 命令 : 


mkdir —/simple gripper tutorial; cd ~/simple gripper tutorial 


(2) 我 们 将 用 一 个 简单 的 空 场景 作为 开始 。 创 建 一 个 场景 文件 ,输入 以 下 命令 : 


gedit —/simple gripper tutorial/gripper. world 
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复制 下 面 SDF 的 内 容 到 名 为 “gripper. world” 的 文件 中 : 


(3) 在 一 / .Gazebo 目录 内 部 创建 模型 。 输 入 以 下 命令 将 模型 文件 写 人 目录 : 


(4) 让 我 们 对 夹 持 器 的 基本 结构 进行 布局 。 做 到 这 一 点 最 简单 的 方法 是 做 一 个 static 
模型 ,并 将 这 些 模型 一 个 个 添加 到 连 杆 上 。 静 态 模 型 意味 着 仿真 器 启动 时 的 连 杆 不 会 移动 。 
这 将 允许 用 户 启动 仿真 器 ,并 在 增加 关节 前 检查 连 杆 位 置 。 

(5) 创建 model. config 文件 : 


(6) 复制 下 列 内 容 : 


第 4 章 ”Gazebo 在 机 器 人 仿真 中 的 应 用 P 399 


CD 同样 地 ,创建 simple gripper. sdf 文件 : 


(8) 将 下 面 的 代码 复制 到 其 中 : 
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<box> 

<size>0.1 0.20.1</size> 
</box> 

</geometry> 

</collision> 

< visual name = "visual"> 

< geometry > 

Xbox?» 

«size» 0.10.2 0.1«/size» 
X/ box » 

«/geonetry > 

«material 

< script» Gazebo/Green </script> 
</material > 

</visual > 

</link> 

< static» true </static > 
</model > 

</sdf > 


(9) 运行 场景 文件 可 以 看 到 已 经 创建 的 夹 持 器 模型 .输入 以 下 命令 : 


Gazebo ~ /simple_gripper_ tutorial/gripper. world 


如 图 4-9 所 示 ,用 户 将 会 看 到 创建 的 夹 持 器 模型 。 


— = 


图 4-9 创建 的 夹 持 器 模型 
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(10) 经 过 以 上 步骤 ,我 们 成 功 地 对 连 杆 进行 布局 。 下 面 我 们 可 以 通过 对 文件 simple | 
gripper. sdf 的 修改 增加 关节 ,在 </model > 行 之 前 添加 下 列 代码 : 


块 的 列 
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«limit» 

X lower >- 0.4 </lower > 

< upper > 0. 4 «/upper > 
</limit> 

<xyz>0 0 1</xyz> 
</axis> 

</joint> 

< joint name = "palm_riser" type= "prismatic"> 
«child» palm </child> 

< parent > riser </parent > 
<axis> 

<limit> 

< lower > 0 </lower > 

< upper > 0. 9 </upper > 
</limit> 

<xyz>0 0 1</xyz> 
</axis> 

</joint> 


并 且 使 模型 非 静态 : 


<static> false</static> 


1) 再 次 启动 Gazebo ,输入 以 下 命令 : 


Gazebo ~ /simple gripper tutorial/gripper. world 


4.3.9 在 机 器 人 上 构建 夹 持 器 
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2) 在 模型 上 单 击 鼠 标 右键 ,依次 选择 “查看 “关节 ”和 * 查 看”“ 线 框 ”。 如 图 4-9 所 
IR ,新 创建 的 关节 模型 将 显示 在 场景 中 。 
3) 用 户 还 可 以 使 用 的 关节 控制 部 件 控制 每 个 关节 的 力量 。 单 击 抓 取 器 模型 。 然 后 
通过 单 击 GUI 右 侧 的 垂直 手柄 ,将 其 拖 动 到 左 侧 扩大 这 个 小 程序 。 该 插件 展示 了 一 系列 滑 
表 , 其 中 每 一 项 代表 一 个 关节 。 选 中 动力 标签 可 以 使 用 滑 块 将 力 施 加 于 每 一 个 关节 ， 
然后 我 们 可 以 看 到 夹 持 器 在 力 的 作用 下 运动 。 例 如 ,设置 在 palm_riser 的 为 10( 单 位 为 牛 
顿 ), 如 图 4-11 所 示 ,将 会 看 到 夹 持 器 在 力 的 作用 下 运动 。 


接 下 来 说 明 如 何 从 现 有 机 器 人 部 分 , 即 移动 机 器 人 和 夹 取 器 的 基础 上 ,创建 带 夹 持 器 的 


简单 复合 机 器 人 。 


1. 机 器 人 组 件 


启动 Gazebo, 并 确保 用 户 可 以 从 前 面 两 个 部 分 内 容 加 载 模型 。 
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图 4-10 关节 模型 


图 4-11 运动 的 夹 持 器 


1) 移动 基 座 
COD 如 图 4-12 所 示 , 在 完成 制作 移动 机 器 人 部 分 内 容 中 的 每 条 命令 后 ,在 用 户 的 
disposal 上 将 会 产生 带 基 座 的 移动 机 器 人 。 
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图 4-12 带 基 座 的 移动 机 器 人 


(2) 通过 修改 一 /. Gazebo/models/my_robot/model. sdf, 可 以 使 模型 更 大 ,这 将 可 以 容 
纳 我 们 需要 在 该 模型 上 添加 的 夹 持 器 ,输入 以 下 命令 : 


gedit —/.Gazebo/models/my robot/model.sdf 


接 下 来 需要 更 新 内 容 ,使 模型 的 大 小 改变 。 输 入 以 下 内 容重 新 定位 车 轮 信 息 : 


<?xml version- '1.0'?» 

< sdf version- '1.5'» 

< model name = "mobile base"> 
< link name = 'chassis'^ 
<pose>0 0 .25 0 0 0 «/pose» 
< inertial > 

< mass > 20. 0 </mass > 
<pose>-0.1 0 -0.1 0 0 0</pose> 
«inertia» 

< ixx» 0.5 «/ixx» 

< iyy>1.0</iyy> 
Xizz»0.1«/izz» 

</inertia> 

</inertial > 

<collision name = 'collision'> 
< geometry > 
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2) 组 装 复合 机 器 人 
D 要 创建 一 个 附带 了 一 个 简单 夹 钳 的 移动 机 器 人 ,需要 创建 一 个 新 的 模型 目录 ,输入 


接 下 来 编辑 模型 的 配置 文件 .输入 以 下 命令 : 


加 载 如 下 内 容 : 


(2) 接 下 来 ,创建 模型 SDF 文件 ,输入 以 下 命令 : 


加 载 如 下 内 容 : 
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< parent > mobile base: :chassis </parent > 
<child> simple gripper: :riser </child> 
</joint> 

<! — attach sensor to the gripper -一 > 
< include» 

< uri» model: //hokuyo «/uri > 

<pose>1.3 0 0.30 0 0</pose> 
</include> 

< joint name = "hokuyo_joint" type = "fixed"> 
< child» hokuyo: : link </child> 

< parent > simple gripper: :palm</parent> 
</joint> 

</model > 

</sdf > 


(3) 确保 model. config 与 manipulator. sdf 已 经 保存 ,启动 Gazebo 和 通过 插入 选项 卡 
来 产生 模型 ,选择 简单 的 移动 机 器 人 模型 来 重新 加 载 以 上 模型 。 如 图 4-13 所 示 , 用 户 可 以 
看 到 组 装 得 到 的 移动 机 器 人 模型 。 


图 4-13. 组 装 得 到 的 移动 机 器 人 模型 


4.3.10 dX £p 


本 部 分 内 容 将 介绍 如 何 将 模型 嵌入 另 一 个 模型 中 ,以 创建 模型 的 集合 。 
1. RERE 
在 “制作 模型 "部 分 内 容 中 可 以 看 到 ,一 个 模型 SDF 是 由 连 杆 (links) 和 关节 (joints) 集 
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合 组 成 的 。 从 SDF 1.5 开始 ,SDF 的 < model > 元 素 已 经 扩展 为 支持 自 引 用 ,这 意味 着 允许 
元 素 < model >R. YE Gazebo 7 中 添加 了 加 载 嵌 套 < model > 元 素 的 支持 。 
FERRER SDF 的 一 个 基本 示例 : 
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此 模型 SDF 由 两 个 连 杆 (link_00 link_01) 和 一 个 嵌 套 模型 (model_01) 组 成 。 由 于 
Gazebo 中 的 模型 只 是 一 个 可 以 容纳 多 个 物体 的 抽象 容器 ,因此 在 Gazebo 中 加 载 此 模型 将 
导致 在 物理 引擎 中 只 创建 两 个 刚体 ,一 个 用 于 球形 连 杆 , 另 一 个 用 于 嵌 套 方形 连 杆 。 默 认 情 
况 下 ,它们 不 会 像 同 一 模型 中 的 其 他 连 杆 一 样 发 生 自 冲突 。 在 GUI 客户 端 上 ,用 户 将 看 到 
一 个 球体 和 一 个 盒子 并 排放 置 ,并 且 不 会 发 现 谋 套 模 型 和 连 杆 之 间 的 任何 视觉 差异 。 

2. 关节 

在 嵌 套 模型 中 ,在 连 杆 之 间 创 建 关节 也 是 可 行 的 。 这 是 一 个 可 以 添加 到 上 面 的 模型 
SDF 的 关节 的 示例 : 


< joint name = "joint 00" type = "revolute"> 
< parent > link 00 </parent > 

«child» model 01::link 01 </child> 
«pose? 0.0 0.00.0 0.0 0.0 0.0 </pose> 
«axis? 

<xyz>1.0 0.0 0.0 </xyz > 

</axis> 

</joint> 


这 个 关节 SDF 753 VT AR Dn edic A ERRE model > 元 素 。 然 后 在 球形 和 箱 形 
连 杆 之 间 将 会 形成 旋转 关节 。 注 意 < parent > 和 < child > 名 称 的 范围 ; 除了 顶级 模型 prefix 
外 , 嵌 套 模型 的 连 杆 的 参考 均 需 要 被 设 定 范围 。 

3. 关于 包含 SDF 元 素 的 注意 事项 

嵌入 模型 可 以 使 用 < include > 元 素 的 用 法 。 

该 < include > 元 素 通过 从 包括 的 模型 中 取得 所 有 连 杆 ,并 将 它们 嵌入 到 父 模型 中 来 工 
作 。 这 种 方法 的 缺点 是 在 模型 显示 过 程 中 被 修改 , 即 保 存 场景 文件 时 将 导致 一 个 模型 
< model > 元 素 的 所 有 连 杆 结合 在 一 起 而 没有 保存 < include > 标签 。 这 是 典 套 < model > 元 素 
设计 来 解决 的 缺点 之 一 。 

另 一 方面 ,<include > 元 素 是 一 个 简单 有 效 的 解决 方案 ,只 需要 一 个 SDF 文件 的 引用 来 
创建 模型 组 合 即 可 。 未 来 的 工作 将 考虑 < model > 使 用 此 功能 扩展 嵌 套 的 SDF 元 素 。 


4.3.11 模型 编辑 器 


本 部 分 内 容 介绍 使 用 模型 编辑 器 创建 模型 的 过 程 。 

CD 打开 模型 编辑 器 。 

启动 Gazebo, 在 Edit 菜单 上 , 转 到 Model Editor 或 单 击 Ctrl+M 组 合 键 打开 编辑 器 。 

(2) 图 形 用 户 界面 。 

该 编辑 器 由 以 下 2 部 分 组 成 : 

左 侧 的 选项 板 有 两 个 选项 卡 。 该 Insert 选项 卡 允 许 用 户 将 零件 ( 连 杆 和 其 他 模型 ) 插 入 
场景 中 以 构建 模型 。 该 Model 选项 卡 显示 构成 用 户 正在 构建 的 模型 的 所 有 零件 的 列表 。 
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右 侧 的 3D 视图 中 ,用 户 可 以 看 到 模型 的 预览 ,并 与 能 够 编辑 其 属性 和 连 杆 之 间 创 建 关 
。 顶 部 工具 栏 上 的 GUI 工具 可 用 于 操作 3D 视图 中 的 关节 和 连 杆 。 

1. 添加 连 杆 

1) 添加 简单 的 形状 

模型 编辑 器 有 3 个 简单 的 原始 几何 体 ,用 户 可 以 插入 到 3D 视图 中 以 创建 模型 的 连 杆 。 

COD 在 调 色 板 上 , 单 击 在 简单 形状 下 的 boxes, phere sk cylinder 图 标 。 

(2) 将 鼠标 光标 移动 到 3D 视图 上 以 查看 视觉 效果 ,然后 在 任何 位 置 单 击 /释放 将 其 添 


加 到 模型 中 。 


网 


网 


注意 : 用 户 可 以 按 Esc 键 取消 添加 附加 到 鼠标 光标 的 当前 连 杆 。 

2) 添加 网 格物 体 

添加 网 格 文件 的 操作 如 图 4-14 所 示 , 它 的 具体 步骤 为 : 

CD 单 击 自 定义 形状 Add 下 的 按钮 ,将 弹出 一 个 对 话 框 ,让 用 户 从 中 找到 要 添加 的 
格 。 

(2) 单 击 Browse 按钮 ,使 用 文件 浏览 器 在 本 地 计算 机 上 查找 网 格 文件 。 如 果 用 户 知道 
格 文件 的 路 径 , 用 户 可 以 直接 在 Browse 按钮 旁边 的 文本 字段 框 中 输入 。Note Gazebo H 


前 仅 支 持 导 入 COLLADA(dae) ,STereoLithography(stD fll Scalable Vector Graphics(svg) 
xt. 


Simple Shapes. 


Custom Shapes 


Model Database 
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L^ 


图 4-14 添加 网 格 文件 操作 
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G) 单 击 Import 以 加 载 网 格 文件 。 然 后 ,将 其 添加 到 3D 视图 。 

2. 创建 关节 

模型 编辑 器 支持 在 正在 编辑 的 模型 中 的 连 杆 之 间 创 建 多 种 类 型 的 关节 。 过 程 如 下 : 

(1) 单 击 joint 工具 栏 上 的 图 标 。 这 将 弹出 创建 关节 的 对 话 框 ,用 户 可 以 指定 要 创建 的 
关节 的 不 同属 性 。 正 如 在 对 话 框 中 看 到 的 ,默认 的 关节 类 型 是 Revolute 关节 。 

(2) 将 鼠标 移动 到 用 户 要 创建 关节 的 连 杆 上 ,这 时 它 将 会 变 亮 , 单 击 选 中 它 。 这 个 连 杆 
将 是 关节 的 父 连 杆 。 

(3) 然后 ,将 鼠标 移动 到 用 户 要 成 为 关节 的 子 连 杆 的 连 杆 。 如 图 4-15 所 示 , 单 击 它 可 
以 看 到 连接 两 个 连 杆 的 彩色 线 和 附加 到 子 连 杆 的 关节 视图 。 


图 4-15 关节 视图 


用 来 表示 关节 的 线 是 彩色 编码 的 ,不 同 的 关节 类 型 对 应 着 不 同 的 颜色 。 关 节 的 视觉 信 
息 由 RGB 轴 组 成 ,这 有 助 于 给 出 关节 的 坐标 框架 的 想法 。 黄 色 箭 头 表 示 接 头 的 主轴 。 例 
如 ,在 旋转 接头 的 情况 下 ,这 是 旋转 轴线 。 

(4) 在 “关节 创建 "对话 框 中 指定 所 有 所 需 的 关节 属性 后 , 单 击 Create 按钮 完成 关节 
创建 。 

注意 : 用 户 可 以 按 Esc 键 在 任意 时 间 取 消 创建 过 程 。 
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3. 编辑 模型 

编辑 模型 时 要 小 心 : 编辑 器 目前 无 法 撤销 用 户 的 操作 ; 所 有 测量 均 以 米 为 单位 。 

1) 编辑 连 杆 

模型 编辑 器 支持 编辑 连 杆 ,并 能 在 SDF 中 找到 这 些 属性 。 

编辑 连 杆 的 属性 的 方法 为 : 双击 连 杆 或 右键 单 击 并 选择 Open Link Inspector。 将 出 现 
一 个 对 话 框 窗口 ,如 图 4-16 所 示 , 其 中 包含 Link, Visual 和 Collision 属性 选项 卡 。 例 如 , 尝 
试 更 改 连 杆 姿势 和 颜色 。 完 成 后 , 单 击 OK 按钮 关闭 检查 器 。 


图 4-16 连 杆 属性 设置 对 话 框 


注意 : Gazebo 6 十 支持 编辑 连 杆 、 视 觉 和 碰撞 。 编 辑 传感器 和 插件 的 能 力 将 在 以 后 的 
版 本 中 实现 。 

2) 编辑 关节 

如 前 所 述 ,关节 属性 也 是 可 以 编辑 的 。 这 些 是 用 户 在 关节 SDF 中 可 以 找到 的 属性 。 

编辑 关节 属性 的 方法 为 : 双击 连接 连 杆 或 右键 单 击 它 并 选择 Open Joint Inspector。 关 
节 编 辑 器 如 图 4-17 所 示 。 例 如 ,尝试 更 改 关节 姿势 和 关节 类 型 。 完 成 后 , 单 击 OK 按钮 关 
闭 检查 器 。 

4. 保存 模型 

保存 将 为 用 户 的 模型 创建 一 个 目录 ,SDF 和 配置 文件 。 这 里 以 一 个 简单 的 例子 作为 说 
明 : 创建 一 个 简单 的 汽车 ,并 保存 。 如 图 4-18 所 示 ,汽车 将 有 一 个 盒 底 盘 和 四 个 可 以 滚动 
的 轮子 ,每 个 滚轮 将 通过 旋转 接头 连接 到 底盘 。 

如 果 用 户 对 所 创建 的 模型 感到 满意 ,请 转 到 Model 左 侧面 板 中 的 标签 ,并 为 其 命名 。 
保存 模型 ,请 选择 File 菜单 ,然后 单 击 顶 部 菜单 中 的 Save As( 按 钮 或 单 击 Cul-- S fO. dn 
图 4-19 所 示 , 将 出 现 一 个 对 话 框 ,用 户 可 以 在 其 中 选择 模型 的 位 置 。 
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图 4-17 关节 对 话 框 


图 4-18 汽车 模型 
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图 4-19 模型 设置 对 话 框 
5. 退出 
完成 创建 模型 并 保存 后 , 转 到 File 然后 选择 Exit Model Editor。 如 图 4-20 所 示 , 用 户 
的 模型 将 显示 在 主 窗口 中 。 


图 4-20 主 窗口 中 的 汽车 模型 


6. 编辑 现 有 模型 

如 果 用 户 不 需要 重新 创建 一 个 模型 ,那么 用 户 可 以 在 仿真 中 编辑 模型 的 属性 。 

编辑 当前 模型 的 方法 为 : 确保 用 户 已 保存 创建 的 模型 ,并 退出 模型 编辑 器 。 或 者 ,从 一 
个 新 的 Gazebo 实例 开始 。 从 Insert 左 侧 的 选项 卡 插入 模型 。 例 如 ,让 我 们 插入 一 个 
Simple Arm。 右 键 单 击 刚 刚 插入 的 模型 ,并 选择 Edit Model。 生 成 模型 如 图 4-21 所 示 。 现 
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在 用 户 在 模型 编辑 器 中 ,用 户 可 以 向 模型 中 添加 新 连 杆 或 编辑 现 有 连 杆 。 


图 4-21 简单 的 机 械 臂 模型 


4.3.12. fx fna 


本 部 分 内 容 创建 一 个 仿真 场景 ,一 个 简单 的 盒子 ,在 重复 循环 10s 的 动画 ,使 它 在 地 面 
上 滑动 。 本 部 分 内 容 还 演示 了 使 用 Gazebo 可 执行 文件 或 用 户 自己 的 自 定 义 可 执行 文件 查 
看 ,访问 和 与 仿真 交互 的 几 种 不 同方 法 。 仿 真 的 盒子 传输 自己 的 位 姿 ,并 且 创 建 回调 来 接收 
姿势 并 打印 出 盒子 的 位 置 和 时 间 戳 。 

1. 建立 

创建 工作 目录 ,输入 以 下 的 命令 : 


mkdir 一 /Gazebo_animatedbox tutorial 
cd —/Gazebo animatedbox tutorial 


2. 动画 框 代码 

(1) 将 animated. box. cc. independent listener. cc. integrated. main, cc. CMakeLists 
. txt 和 animated box. world 复制 到 当前 目录 中 。 

(2) 在 OSX 系统 的 Gazebo 上 ,用 户 可 以 替换 wget 为 curl -OL, 


wget http://bitbucket. org/osrf/Gazebo/raw/Gazebo8/examples/stand —alone/animated _ box/ 
animated box.cc 

wget http://bitbucket. org/osrf/Gazebo/raw/GazeboB8/examples/stand _ alone/animated _ box/ 
independent listener.cc 

wget http://bitbucket. org/osrf/Gazebo/raw/GazeboB8/examples/stand _ alone/animated _ box/ 
integrated main.cc 

wget http://bitbucket. org/osrf/Gazebo/raw/Gazebo8/examples/stand ^ alone/animated _ box/ 
CMakeLists.txt 


420 A| 机 器 人 仿真 与 编程 技术 


wget http://bitbucket. org/osrf/Gazebo/raw/Gazebo8/examples/stand  alone/animated _ box/ 
animated box. world 


(3) 构建 插件 ,输入 以 下 命令 : 


mkdir build 
cd build 
cmake ../ 
make 


(4) 确保 Gazebo 可 以 稍 后 加 载 插件 ,输入 以 下 命令 : 


exportGAZEBO_PLUGIN_PATH = 'pwd': $ GAZEBO PLUGIN PATH 


3. 用 Gazebo 仿真 
此 示例 演示 如 何 运 行使 用 插件 的 Gazebo 可 执行 文件 ,输入 以 下 命令 : 


cd —/Gazebo animatebox tutorial 
Gazebo animated box.world 


在 另 一 个 终端 中 ,使 用 "gz topic” 用 户 界面 查看 姿势 ,输入 以 下 命令 : 


gz topic - v /Gazebo/animated box world/pose/info 


用 户 应 该 会 看 到 一 个 图 形 界 面 , 显 示 盒子 的 姿势 。 

4. 连接 自己 的 可 执行 文件 到 仿真 器 

首先 是 确保 Gazebo 没有 运行 ; 然后 如 上 所 述 ,启动 Gazebo, 最 后 运行 连接 到 Gazebo 的 独 
立 侦 听 器 可 执行 文件 。 独 立 侦 听 器 接收 盒子 的 位 置 和 时 间 戳 ,并 打印 出 来 ,运行 以 下 命令 ; 


cd —/Gazebo animatebox tutorial 
Gazebo animated box. world & ./build/independent listener 


5. 运行 仿真 并 连接 自己 的 可 执行 文件 

同样 地 ,需要 确保 Gazebo 没有 运行 。 

integrated_main 示例 演示 了 以 下 内 容 : 

。 开始 盒子 仿真 。 

。 将 侦 听 器 连接 到 仿真 ,作为 同一 可 执行 文件 的 一 部 分 。 
。 侦 听 器 获取 时 间 截 和 位 置 ,然后 打印 出 来 。 

运行 integrated_main ,输入 以 下 命令 : 


cd —/Gazebo animatebox tutorial 
./build/integrated main animated box. world 
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如 需 查 看 仿真 ,输入 以 下 命令 : 

gzclient 

6. 源 代 码 

对 源 代码 的 说 明 如 下 : 

independent listener.cc 

它 是 一 个 可 执行 文件 ,将 连接 到 正在 运行 的 仿真 ,从 姿势 信息 主题 接收 更 新 ,并 打印 对 
SE. 


integrated main.cc 


它 是 一 个 可 执行 文件 将 创建 一 个 仿真 ,从 姿势 信息 主题 接收 更 新 ,并 打印 对 象 位 置 。 


animated box. cc 


它 是 一 个 共享 库 插入 定义 仿真 动画 组 件 , 用 来 移动 了 场景 上 的 盒子 。 


animated box. world 


它 是 一 个 XML 文件 ,定义 仿真 物理 场景 空间 和 其 中 的 单个 盒子 。 


CMakeLists. txt 


它 是 一 个 CMake 构建 脚本 。 


4.3.13 三 角 网 格 的 惯性 参数 


一 个 准确 的 仿真 需要 物理 上 的 合理 惯性 参数 : 质量 、 质 心 位 置 和 所 有 连 杆 的 惯性 矩 矩 
阵 。 如 果 用 户 有 连 杆 组 成 的 3D 模型 ,本 部 分 内 容 将 指导 用 户 完 成 获取 和 设置 这 些 参数 。 
假设 均匀 体 (均匀 质量 密度 ) ,下面 展 示 如 何 使 用 免费 软件 MeshLab 获得 惯性 数据 。 用 户 还 
可 以 使 用 商业 产品 SolidWorks 来 计算 这 些 信息 。 

1. 惯性 参数 说 明 

(1) 质量 最 容易 通过 称 重 物体 来 测量 。 它 是 一 个 标量 ,在 Gazebo 中 公斤 (kg ) 为 默认 单 
位 。 对 于 3D 均匀 网 格 ,通过 计算 几何 体积 并 乘 以 密度 来 计算 质量 。 

(2) 质心 是 加 权 质 量 矩 的 和 为 零 的 点 。 对 于 一 个 均匀 的 物体 ,这 相当 于 几何 形 心 。 此 
参数 是 一 个 长 度 为 3 的 向 量 , 具 有 位 置 的 单位 。 

(3) 转动 惯量 代表 质量 在 刚性 体 的 空间 分 布 。 它 取决 于 具有 单位 为 “质量 x 长度” 的 
物体 的 质量 .尺寸 和 形状 。 转 动 惯量 可 以 表示 为 对 称 正 定 3X3 矩阵 的 分 量 , 具 有 3 个 对 角 
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元 素 和 3 个 唯一 的 非 对 角 元 素 。 每 个 惯性 矩阵 的 定义 都 和 坐标 系 和 坐标 轴 相 关 。 将 矩阵 对 
角 化 将 会 产生 其 主要 转动 惯量 (特征 值 ) 和 其 主轴 的 方向 (特征 向 量 )。 

转动 惯量 与 质量 成 比例 ,但 是 与 大 小 成 非 线 性 关系 。 此 外 ,对 主要 惯量 的 相对 值 存在 约 
东 , 这 通常 使 得 估计 转动 惯量 比 估 计 质 量 或 质量 中 心 位 置 更 难 。 这 个 困难 促使 使 用 软件 工 
具 来 计算 转动 惯量 。 

2. 安装 MeshLab 

用 户 从 官方 网 站 下 载 MeshLab ,并 将 其 安装 在 用 户 的 计算 机 上 。 安 装 后 ,用 户 可 以 在 
MeshLab 中 查看 网 格 (支持 DAE 和 STL 格式 ,这 是 Gazebo/ROS 支持 的 格式 ) 。 

3. 计算 惯性 参数 

(1) 计算 球体 惯性 。 

在 MeshLab 中 打开 网 格 文件 。 对 于 本 示例 ,使 用 sphere. dae 网 格 。 要 计算 惯性 参数 ， 
首先 需要 显示 对 话 框 View-> Show Layer Dialog。 在 窗口 的 右边 部 分 打开 一 个 面板 , 它 分 
成 两 部 分 ,我 们 感 兴趣 的 是 包含 文本 输出 的 部 分 。 

接 下 来 ,用 MeshLab 计算 惯性 参数 。 从 菜单 中 选择 依次 Filters->Quality Measure and 
Computations--Compute Geometric Measures。 对 话 框 的 下 半 部 分 将 显示 有 关 惯 性 测量 的 
一 些 信息 。 球 体 给 出 以 下 输出 : 


Mesh Bounding Box Size 2.000000 2.000000 2.000000 
Mesh Bounding Box Diag 3.464102 

Mesh Volume is 4.094867 

Mesh Surface is 12.425012 

Thin shell barycenter — 0.000000 -0.000000 -0.000000 
Center of Mass is - 0.000000 0.000000 — 0.000000 
Inertia Tensor is : 

| 1.617916 -0.000000 0.000000 | 

| - 0.000000 1.604620  — 0.000000 | 

| 0.000000 - 0.000000 1.617916 | 

Principal axes are : 

| 0.000000 1.000000 0.000000 | 
|-0.711101 - 0.000000 0. 703089 | 

| - 0. 703089 0.000000 -0.711101 | 

axis momenta are : 

| 1.604620 1.617916 1.617916 |() 


(2) 半径 : 球体 的 边界 框 是 边 长 为 2.0 的 立方 体 .这 意味 着 球体 的 半径 为 1. 0。 

G) 体积 : 半径 为 1.0 的 球体 应 具有 4/3 * PI(4. 189) 的 体积 ,其 接近 于 计算 值 4.095。 
它 不 是 精确 值 , 因 为 它 是 三 角形 近似 值 。 

(4) 表面 积 : 表面 积 应 为 4x PI(12. 566) ,接近 计算 值 12. 425。 

(5) 质心 : 质心 给 定 为 原点 (0.0.0) 。 

(6) 惯性 矩阵 : 由 于 球体 的 半径 为 1. 球体 的 惯性 矩阵 (也 称 为 惯性 张 量 ) 应 该 是 对 角 
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的 ,其 主要 转动 惯量 为 2/5 质量 。 它 没有 在 输出 中 明确 说 明 , 而 是 质量 等 于 体积 (默认 密度 
为 1) ,因此 我 们 预计 对 角 和 矩阵 条 目 为 8415 * PI(1. 676)。 在 给 定 精度 下 计算 出 的 惯性 张 量 
将 呈现 对 角 线 形式 ,主要 惯量 在 [1. 604.1. 618] 的 范围 内 ,接近 预期 值 。 


(7) 复制 曲面 : 复制 曲面 需要 记 住 的 一 点 是 ,网 格 内 的 复制 曲面 将 影响 体积 和 转动 惯 
量 的 计算 。 例 如 ,考虑 另 一 个 球形 网 格 : ball. dae。Meshlab 为 此 网 格 提供 以 下 输出 : 


Mesh Bounding Box Size 1.923457 1.990389 1.967965 
Mesh Bounding Box Diag 3.396207 

Mesh Volume is 7.690343 

Mesh Surface is 23.967396 

Thin shell barycenter 0.000265 0.000185 0.000255 
Center of Mass is 0.000257 0.000195 0.000292 
Inertia Tensor is : 

| 2.912301 0.001190 0. 000026 | 

| 0.001190 2.903731 0.002124 | 

| 0.000026 0.002124 2.906963 | 

Principal axes are : 

| 0.108262 -0.895479 0.431738 | 

| - 0.120000 0.419343 0.899862 | 

| 0.986853 0.149229 0.062058 | 

axis momenta are : 

| 2.902563 2.907949 2.912483 | 


此 网 格 大 小 大 致 相同 ,边界 框 尺寸 在 [1.92,1. 99] 范 围 内 ,但 其 计算 差异 近 两 倍 : 

* 体积 : 对 比 7.69 与 4.09。 

。 主要 惯量 : 对 比 [2. 90.2. 91] 与 [1. 60,1. 62]. 

当 用 户 查 看 顶点 和 面 的 数量 ( 列 在 MeshLab 窗口 底部 ) 时 ,不 同 点 为 : 

* sphere. dae: 382 个 顶点 ,760 个 面 。 

。 ball. dae: 362 个 顶点 ,1440 个 面 。 

每 个 网 格 具有 相似 数量 的 项 点 ,但 是 ball. dae 具有 大 约 两 倍 的 面 。 依 次 运行 命令 
Filters—>Cleaning and Repairing—>Remove Duplicate Faces 将 面 数 减少 ball. dae 到 720, 并 
为 体积 (3. 84) 和 主 转动 惯量 (1. 45) 提 供 更 合理 的 值 。 有 意义 的 是 ,这 些 值 稍 小 ,因为 边界 框 
也 稍 小 。 

(8) 缩放 以 提高 数值 精度 。 

Meshlab 当前 打印 具有 6 位 固定 精度 的 几何 信息 。 如 果 网 格 太 小 ,这 可 能 会 大 大 限制 
惯性 张 量 的 精度 ,例如 : 


Mesh Bounding Box Size 0.044000 0.221000 0.388410 
Mesh Bounding Box Diag 0.449043 

Mesh Volume is 0.001576 

Mesh Surface is 0.136169 
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Thin shell barycenter 一 0.021954 0.008976 0.012835 
Center of Mass is - 0.021993 0.001259 0.001489 
Inertia Tensor is : 

0.000008 -0.000000 -0.000000 | 
— 0. 000000 0.000001  — 0.000000 | 
—0.000000  — 0.000000 0.000007 | 
Principal axes are : 

0.999999 0.000166 0.001241 | 
-0.000113 0.999104 - 0.042310 | 
—0.001247 0.042310 0.999104 | 
axis momenta are : 

0.000008 0.000001 0.000007 | 


似乎 我 们 得 到 了 寻找 的 东西 。 但 是 当 用 户 仔 细 观 察 , 用 户 不 难 发 现 一 个 漏洞 : 输出 
最 多 6 位 的 十 进 制 数 。 因 此 ,我 们 失去 了 惯性 张 量 中 的 大 部 分 有 价值 的 信息 。 为 了 
服 惯性 传感器 的 精度 不 足 ,用 户 可 以 按 比例 放大 模型 ,以 增加 惯性 的 大 小 。 该 模型 可 以 使 
缩放 通过 Filters Normals. Curvatures and Orientation 一 Transform: Scale。 模 型 缩放 
例 设 置 对 话 框 如 图 4-22 所 示 ,在 对 话 框 中 输入 缩放 比例 并 单 击 Apply. 
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图 4-22 模型 缩放 比例 的 设置 


M 


对 于 如 何 选择 缩放 因子 s. MeshLab 使 用 体积 替代 质量 ,其 将 作为 变化 。 此 外 ,惯性 


的 ,因此 缩放 10 倍 或 100 倍 可 能 就 足够 了 。 


长 度 的 平方 具有 附加 依赖 性 ,因此 惯性 矩 将 根据 ”而 改变 。 由 于 s 存在 这 样 大 的 依赖 性 
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O) 获得 质量 中 心 。 

MeshLab 使 用 的 长 度 单位 并 不 总 是 用 户 想 要 的 (Gazebo 为 米 ) 。 但 是 ,通过 查看 Mesh 
Bounding Box Size 条 目 ,用 户 可 以 轻松 地 知道 MeshLab 单位 与 所 需 单位 的 比率 。 例 如 用 
户 可 以 以 所 需 单位 计算 边界 框 大 小 ,并 与 MeshLab 的 大 小 进行 比较 。 

用 Center of Mass 乘 以 计算 的 比率 ,用 户 将 拥有 网 格 质心 的 坐标 。 然 而 ,如 果 用 户 建 模 
的 连 杆 不 是 均匀 的 ,用 户 将 不 得 不 使 用 其 他 方法 (最 可 能 通过 真实 的 实验 ) 计 算 质心 。 

(10). 重新 调整 转动 惯量 值 。 

就 像 质心 位 置 必 须 缩 放 到 正确 的 单位 一 样 ,转动 惯量 也 应 该 按 比 例 缩 放 ,尽管 缩放 因子 
应 该 平方 再 考虑 转动 惯量 的 长 度 的 平方 的 变化 。 此 外 ,惯量 应 乘 以 测量 值 mass, 并 除 以 文 
本 输出 的 计算 体积 。 

(11) 在 URDF 或 SDF 中 填写 标签 。 

下 一 步 是 将 计算 值 记 录 到 包含 机 器 人 的 URDF 或 SDF 文件 (假设 用 户 已 经 具有 机 器 
人 模型 ;如 果 没 有 ,请 按照 前 面部 分 内 容 制 作 模型 ) 。 

在 每 个 连 杆 中 ,用 户 应 该 有 < inertial > 标签 。 它 应 该 具有 如 下 内 容 ( 在 SDF 中 ): 


< link name = 'antenna'> 

< inertial> 

< pose » — 0.022 0.0203 0.02917 0 0 0 </pose > 
< mass > 0. 56 «/nass > 

< inertia> 

< ixx > 0. 004878 «/ ixx > 

< ixy>- 6.2341e - 07 </ixy> 

< ixz >- 7.4538e - 07 </ixz > 

< iyy > 0. 00090164 </iyy> 

< iyz >- 0. 00014394 </iyz > 

< izz > 0. 0042946 </ izz > 

</inertia> 

</inertial > 

€ collision name= 'antenna_collision'> 


</collision> 
< visual name = 'antenna visual' 


</visual > 
</link> 
或 者 (在 URDF n): 


< link name = "antenna"> 
< inertial> 
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«origin rpy- "0 0 0" xyz =" — 0.022 0.0203 0.02917"/> 

«mass value = "0.56"/> 

< inertia ixx = "0.004878" ixy- " —6.2341e- 07" ixz="—7.4538e— 07" iyy - "0.00090164" iyz 
=" — 0. 00014394" izz = "0.0042946"/» 

</ inertial > 

< visual > 


</visual > 
<collision> 


</collision> 
</link> 


其 中 ,< mass > 应 以 千克 为 单位 输入 ,用 户 需 通 过 实验 或 者 规定 它 的 值 。< origin > 或 
< pose > 用 于 输入 质量 中 心 的 位 置 (相对 于 连 杆 的 起 点 ,尤其 是 与 可 视 的 连 杆 或 碰撞 起 点 无 
关 )。 旋 转 元 件 可 以 定义 与 转动 惯量 轴 不 同 的 坐标 系 。 如 果 用 户 已 经 通过 实验 找到 质心 ,请 
填写 此 值 ,否则 填写 由 MeshLab 计算 的 正确 缩放 的 值 。 

< inertia > 标记 包含 用 户 在 上 一 步 中 计算 的 惯性 张 量 。 由 于 矩阵 是 对 称 的 ,只 有 6 个 数 
字 就 足以 表示 它 。 从 MeshLab 的 输出 的 映射 如 下 : 

| ixx ixy ixz | 

| ixy iyy iyz | 

| ixz iyz izz | 


为 了 快速 检查 矩阵 是 否 正确 ,可 以 使 用 对 角 线 条 目 应 具有 最 大 的 正 值 并 ,并 且 非 对 角 线 
数字 应 该 或 多 或 少 接近 零 。 

确切 地 说 ,矩阵 必须 是 正定 的 (使 用 用 户 最 喜欢 的 数学 工具 来 验证 )。 其 对 角 项 也 必须 
满足 三 角 不 等 式 , 即 ,ixx 十 iyy 三 izz,ixx + izz > iyy 和 iyy + izz > ixx. 

(12) 在 Gazebo 上 检查 。 

要 检查 一 切 是 否 正确 ,用 户 可 以 使 用 Gazebo 的 GUI 客户 端 。 

单独 使 用 Gazebo 时 ,运行 Gazebo ,生成 用 户 的 机 器 人 


gz model — f my robot. sdf 
使 用 Gazebo 和 ROS 时 ,首先 运行 Gazebo: 
roslaunchGazebo ros empty world. launch 


生成 用 户 的 机 器 人 (使 用 户 的 机 器 人 的 包 / 名 称 蔡 换 my. robot. my. robot. description, 
MyRobot) : 
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SDF 模型 为 : 


rosrunGazebo ros spawn model — sdf - file 'rospack find my robot description'/urdf/my robot 
. sdf — model MyRobot 


URDF 型 号 为 : 


rosrunGazebo ros spawn model — urdf - file 'rospack find my robot description'/urdf/my robot 
.urdf — model MyRobot 


当 模型 加 载 后 ,暂停 场景 并 删除 ground_plane( 这 不 是 必需 的 ,但 它 通 常 使 调试 更 容 
易 ) 。 转 到 Gazebo 菜单 并 依次 选择 View 一 Inertia。 每 个 连 杆 目前 应 显示 一 个 带 绿色 轴 的 
紫色 框 。 每 个 盒子 的 中 心 与 其 连 杆 的 指定 质心 对 齐 。 箱 的 尺寸 和 方向 对 应 于 具有 相同 惯性 
行为 单位 质量 的 箱 这 可 以 作为 它们 对 应 的 链接 。 虽 然 这 对 于 调试 惯性 参数 很 有 用 ,但 是 我 
们 还 可 以 做 一 件 事 来 使 调试 更 容易 。 

用 户 可 以 临时 将 所 有 连 杆 设置 为 质量 为 1. 0( 通 过 编辑 URDF 或 SDF 文件 )。 然 后 所 
有 的 紫色 框 应 该 或 多 或 少 在 形状 与 其 连 杆 的 边框 相同 。 这 种 方式 ,用 户 可 以 很 容易 地 检测 
问题 ,如 错 放 的 质心 或 错误 旋转 的 惯性 矩阵 。 当 用 户 完成 调试 时 ,不 要 忘记 输入 正确 的 
质量 。 

要 修正 错误 旋转 的 惯性 矩阵 (常见 错误 ), 只 需 交 换 模型 文件 中 的 ixx、iyy 和 izz 条 目 ， 
直到 紫色 框 与 其 连 杆 对 齐 。 然 后 用 户 显然 也 必须 适当 地 交换 ixy、ixz 和 iyz 值 ( 当 用 户 交换 
ixxeriyy 时 ,用 户 应 该 取消 ixy 和 交换 ixzeriyz) 。 

4. 进一步 改进 

1) 简化 模型 

MeshLab 仅 计算 闭合 形状 的 正确 惯性 参数 。 如 果 用 户 的 连 杆 是 开放 的 ,或 者 它 是 一 个 
非常 复杂 或 凹陷 的 形状 。 在 计算 惯性 参数 之 前 简化 模型 可 能 是 一 个 好 选择 (例如 在 
Blender)。 或 者 ,如 果 用 户 的 模型 具有 碰撞 形状 ,请 使 用 它们 代替 全 分 辩 率 模型 。 

2) 非 均 质 体 

对 于 非常 不 均匀 体 , 本 部 分 内 容 可 能 不 工作 ,涉及 两 个 问题 。 第 一 个 是 MeshLab 假设 
均匀 密度 的 身体 。 另 一 个 是 MeshLab 计算 相对 于 计算 质心 的 惯性 传感器 。 然 而 ,对 于 非常 
不 均匀 体 ,计算 的 质心 将 远离 真实 质心 :因此 计算 的 惯性 张 量 可 能 是 错误 的 。 

一 个 解决 方案 是 将 用 户 的 连 杆 细 分 为 更 均匀 的 部 分 ,并 使 用 固定 关节 连接 它们 ,但 这 并 
非 永远 可 行 。 唯 一 的 其 他 解决 方案 是 通过 实验 找 出 惯性 张 量 , 这 肯定 需要 大 量 的 时 间 和 
精力 。 

我 们 已 经 展示 了 为 机 器 人 模型 获得 正确 惯性 参数 的 过 程 , 如 何 将 它们 输入 到 URDF 或 
SDF 文件 中 的 方法 ,以 及 如 何 确保 正确 输入 参数 的 方法 。 
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4.3.14 图 层 可 见 性 

使 用 Gazebo 6, 可 以 向 仿真 中 的 视觉 元 素 添 加 元 数据 。 本 部 分 内 容 解释 如 何 向 可 视 化 
添加 图 层 元 数据 ,以 便 用 户 可 以 通过 图 形 界面 控制 哪些 图 层 可 见 。 

1. 在 SDF 上 分 配 图 层 

目前 ,图 层 由 数字 标识 。 在 用 户 的 模型 SDF 文件 中 ,在 每 个 < visual > 标记 下 ,用户 可 以 
添加 < meta > 标记 来 记录 元 信息 ,然后 为 < layer > 层 号 添加 标记 如 下 : 

< visual name = 'visual 0'> 

<meta> 


< layer > 0 </layer > 
</meta> 


EI 
没有 分 配 图 层 的 视觉 效果 不 能 进行 可 见 性 切换 ,并 且 始 终 可 见 。 


2. 可 视 化 图 层 
一 个 场景 示例 分 布 在 Gazebo 中 。 用 户 可 以 加 载 此 场景 ,输入 以 下 命令 : 


Gazebo worlds/shapes_layers. world 


用 户 可 以 通过 Layers 左 侧面 板 上 的 标签 切换 每 个 图 层 的 可 见 性 。 如 果 仿 真 中 的 图 层 
没有 可 视 物 ,图 层 标签 页 就 是 空 。 


4.4 Gazebo 中 的 模型 编辑 器 
该 部 分 描述 如 何 使 用 模型 编辑 器 构建 和 修改 机 器 人 。 
4.4.1 模型 编辑 器 
该 部 分 内 容 与 构建 机 器 人 部 分 的 模型 编辑 器 内 容 相同 。 故 不 熬 述 。 


4.4.2 SVG 文件 


本 部 分 内 容 描述 了 得 到 SVG 文件 ( 即 2D 图 像 ) 的 过 程 ,以 便 为 Gazebo 中 的 模型 创建 
3D 网 格 。 此 过 程 类 似 于 在 Inkscape 3X Illustrator 这 样 的 程序 中 设计 模型 的 一 部 分 。 并 将 
向 用 户 介绍 如 何在 Inkscape 中 将 自 定义 滚轮 制作 为 . svg, 并 将 其 导入 Gazebo, AE I Mh 
加 到 机 器 上 。 

Gazebo 中 有 许多 SVG 编辑 器 。 本 部 分 内 容 将 使 用 开源 Inkscape 程序 ,这 是 本 部 分 内 
容 中 使 用 的 轮子 的 SVG 文件 。 
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1. 文件 准备 

启动 Inkscape。 这 将 创建 一 个 空白 文档 。 如 图 4-23 所 示 ,首先 让 我 们 更 改 文档 大 小 ， 
以 便 更 好 地 适应 轮子 : 在 File Document properties menu 下 ,选择 Page 标签 ,将 文档 大 小 
更 改 为 自 定义 大 小 100.0X100.0 毫米 。 

如 图 4-24 所 示 为 另 一 个 文件 属性 设置 对 话 框 。 在 同一 对 话 框 中 ,选择 Grids 选项 卡 ， 
按 New 按钮 创建 自 定 义 网 格 。 最 后 ,检查 Enabled. Visible 和 Snap to visible grid lines 
only 选项 。 更 改 Spacing X 和 Spacing Y 为 10。 


Page Guides Grids Snap Color Management Scripting Page Guides Grids Snap Color Management Scripting 
General Creation 
Defadtunts: [px > Rectangular grid H New 
Background: mom Defined grids 
Page Size m grid2985 
^ 2100x297.0mm. Rectangular grid 
US Letter &5x11.0in Ml Enabled 
US Legal 85x 140in A Visible 
US Executive 72x 05in M Snap to visible grid lines only 
Orientation: Gridunits: mm = 
Custom size COPIE 
Width: | 100.00 H Units mm : 
OriginY: [0.0000 
Height: [100.00 SpacingX: |10.0000 
Y Resize page to content... spacing: [10.0000 = 
em oso Crid Une color. mess 
Left: [000 : mige: [0.00 
Major grid Une color. ENKENEEENNNER 
Bottom: |0.00 
Major grid line every; [5 
Resize page to drawing or selection Show dots instead of lines 
Border Remove 
WI Show page border 
D Border ontop of drawing 
E Show border shadow. 
Border color: mm 
图 4-23 文件 属性 设置 对 话 框 一 图 4-24 文件 属性 设置 对 话 框 二 


上 述 操作 完成 后 ,如 图 4-25 所 示 ,用 户 应 该 得 到 一 个 绘制 初始 界面 。 

2. 绘制 

用 户 可 以 使 用 不 同 的 工具 (例如 笔 、 文 本 、 星 星 和 形状 等 ) 来 创建 几何 体 。 在 这 个 例子 
中 ,车 轮 是 由 圆 形 组 成 的 ( 按 下 Shift 键 ,用 户 可 以 从 中 心 开 始 创 建 圆 形 , 并 使 用 Ctrl 键 允许 
保持 形状 的 圆 度 )。 用 户 可 以 将 形状 组 合 在 一 起 .确保 路 径 是 封闭 的 ,并 且 部 件 具 有 适当 的 
厚度 ,绘制 的 效果 如 图 4-26 所 示 。 

注意 : 一 个 棒 形 图 或 两 个 彼此 接触 的 圆圈 不 会 产生 有 效 的 Gazebo 模型 。SVG 路 径 必 
须 创 建 具有 孔 的 封闭 轮廓 ,其 中 孔 不 能 接触 轮 廊 或 其 他 孔 。 孔 内 的 孔 被 视 为 实心 零件 (也 可 
以 有 孔 )。 
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图 4-25 绘制 初始 界面 图 4-26 绘制 界面 一 


Gazebo 只 能 导入 paths, 但 通过 Inkscape 很 容易 将 任何 形状 转换 为 路 径 。 从 Edit 菜单 
选择 Select All。 然 后 依次 选择 Path Object to Path 菜单 项 。 如 图 4-27 所 示 ,这 将 把 每 个 
对 象 转换 为 单独 的 路 径 和 子路 径 。 这 种 转换 是 不 可 逆 的 ,所 以 如 果 用 户 将 文本 转换 成 路 径 ， 
用 户 将 无 法 改变 文本 。Gazebo 不 支持 分 组 ,可 以 使 用 Object 菜单 中 的 Ungroup 分 离 路 
径 组 。 

3. 保存 绘图 

使 用 菜单 中 的 Save 选项 File ,将 图 形 保存 为 SVG 文件 , 稍 后 可 以 在 Gazebo 中 使 用 。 

4. 创建 Gazebo 模型 

SDF 格式 不 直接 支持 SVG; 它 支 持 多 线 2D。Gazebo 模型 编辑 器 具有 从 SVG 文件 中 
提取 多 边 形 线 的 导入 机 制 ,并 将 它们 保存 为 SDF 模型 文件 。 启 动 Gazebo, M Edit 菜单 中 选 
择 Model Editor 以 进入 Gazebo 模型 编辑 器 模式 (与 仿真 模式 相反 )。 如 图 4-28 所 示 , 按 下 
选项 卡 部 分 中 的 Add 按钮 ,可 以 添加 相应 的 模型 。 


O 


OU 二 二 | 站 页 .| 人、 


eV 


EDT TA Ed 
* #0 O00 nr 


日 
H 


图 4-27 绘制 初始 界面 二 图 4-28 模型 的 添加 
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Import Link 对 话 框 如 图 4-29 所 示 , 从 中 用 户 可 以 通过 按 Browse 按钮 选择 SVG 文件 。 
选择 文件 后 , 按 Import 按钮 打开 Extrude Link 对 话 框 。 

该 对 话 框 允许 用 户 设置 模型 的 参数 : 

(1) 厚度 : 连 杆 的 粗细 。 这 对 应 于 Z 轴 中 的 模型 高 度 。 对 于 右 侧 所 示 的 SVG 路 径 , 生 


成 轴线 从 屏幕 向 外 。 
(2) 分 辩 率 : SVG 中 像素 点 的 个 数 是 以 米 为 单位 来 统计 的 。 默 认 值 (3543. 3px/m) 对 


应 于 90dpi( 每 英寸 点 数 ) ,这 是 多 个 编辑 器 (包括 Inkscape) 的 默认 分 辩 率 。 当 显示 单位 为 米 
时 ,如 果 用 户 的 模型 在 Inkscape 中 显示 为 用 户 所 需 的 大 小 , 则 不 应 更 改 分辩 率 值 。 

(D 每 个 片段 的 样本 : 这 表示 SVG 中 的 每 个 曲线 路 径 将 会 有 多 少 个 片段 。 片 段 越 多 ， 
用 户 的 连 杆 就 越 复杂 。 它 不 会 改变 任何 直线 路 径 。 

在 右 侧 ,用 户 可 以 看 到 从 SVG 提取 的 路 径 。 红 点 是 生成 的 3D 模型 的 三 角形 的 顶点 。 
将 车 轮 的 厚度 设置 为 0.025m, 然 后 按 OK 按钮 。 用户 的 新 连 杆 应 显示 在 3D 视图 中 。 这 样 
就 创建 了 一 个 新 的 连 杆 , 它 带 有 一 个 默认 的 碰撞 形状 , 它 是 生成 的 3D 网 格 的 副本 。 

接 下 来 ,从 File 菜单 中 选择 Exit Model Editor。 如 图 4-30 所 示 ,Gazebo 将 注意 用 户 将 
新 模型 保存 到 磁盘 。 按 Save and Exit 退出 对 话 框 上 的 按钮 ,Save Model 将 出 现 对 话 框 。 


O Save Model 


Import Link 


Author 


Browse 


IDefaultName! 


Cancel| [Import 


图 4-29 Import Link 对 话 框 图 4-30 “保存 模型 "对 话 框 


将 新 模型 的 名 称 设置 为 “HollowWheel” ,填写 Advanced Options 部 分 下 的 信息 ,并 按 
Save 按钮 保存 。 


4.5 场景 文件 的 创建 


这 部 分 内 容 描述 如 何 创建 一 个 可 以 仿真 机 器 人 的 环境 。 
4.5.1 创建 一 个 场景 
接 下 来 介绍 创建 包含 静态 和 动态 对 象 的 场景 的 过 程 。 
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1. 术语 

场景 (world) : 用 于 描述 机 器 人 和 物体 (如 建筑 物 、 桌 子 和 灯光 ) 的 集合 以 及 包括 天 空 、 
环境 光 和 物理 属性 的 全 局 参数 的 术语 。 

静态 : 标记 为 静态 的 实体 (< static > true </static > 在 SDF 中 具有 元 素 的 实体 ) 是 仅 具 
有 碰撞 几何 的 对 象 。 所 有 不 打算 移动 的 对 象 应 该 标记 为 静态 ,这 是 一 种 增强 性 能 的 方法 。 

动态 : 标记 为 动态 的 实体 (在 SDF 中 缺少 < static > 元 素 或 < static > 设置 为 false) 是 具有 
惯性 和 碰撞 几何 结构 的 物体 。 

2. 添加 对 象 

启动 Gazebo 时 ,用 户 应 该 看 到 一 个 只 有 地 板 的 场景 。Gazebo 提供 了 两 种 将 对 象 添加 
到 Gazebo 的 机 制 。 第 一 种 是 位 于 泻 染 窗口 上 方 的 一 组 简单 形状 ,如 图 4-31 所 示 。 


图 4-31 添加 对 象 窗口 一 


第 二 种 是 通过 模型 数据 库 , 如 图 4-32 所 示 , 可 以 通过 选择 左上 角 的 Insert 选项 卡 访问 。 

3. 添加 简单 形状 

通过 单 击 泻 染 窗口 上 方 的 相应 图 标 ,可 以 将 立方 体 \、 球 体 和 圆柱 体 添加 到 场景 中 。 每 个 
形状 的 单位 大 小 如 下 : 

。 立方 体 : 1ImXlmXlm 

。 球体 : 直径 1m 

* Bf: 直径 lm, 长 1m 

选择 立方 体 图 标 , 然 后 将 鼠标 移动 到 演 染 窗口 。 用 户 将 看 到 一 个 随和 鼠标 移动 的 立方 体 。 
当 用 户 对 立方 体 的 位 置 感到 满意 时 . 单 击 鼠 标 左 键 。 

对 球体 和 圆柱 体重 复 相同 的 步骤 。 操 作 完 成 后 ,如 图 4-33 所 示 , 用 户 应 该 建立 出 一 个 
类 似 这 样 的 场景 。 
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图 4-32 添加 对 象 窗口 二 


图 4-33 添加 了 简单 形状 的 场景 


4. 从 模型 数据 库 添加 模型 
Gazebo 的 模型 数据 库 是 所 有 类 型 模型 的 存储 库 ,包括 机 器 人 、 表 格 和 建筑 物 。 
CD 选择 左上 角 的 Insert 选项 卡 以 访问 模型 数据 库 。 


模型 列表 根据 其 当前 位 置 分 为 几 个 部 分 。 每 个 部 分 都 标记 有 路 径 或 URI。 选 择 位 于 
远程 服务 器 上 的 对 象 将 会 自动 下 载 模型 并 存储 到 一 /. gazebo/ models. 


(2) 尝试 添加 各 种 模型 到 场景 。 下 载 模型 时 要 有 了 耐心 ,因为 有 些 可 能 体积 很 大 。 
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5. 位 置 模型 
如 图 4-34 所 示 ,可 以 通过 平移 和 旋转 工具 改变 每 个 模型 的 姿势 。 


图 4-34 平移 和 旋转 工具 按钮 


6. 平移 

平移 工具 允许 用 户 沿 z、y 和 < 轴 移 动 对 象 。 现 在 选择 此 工具 (或 按 工 键 ), 然 后 单 击 要 
移动 的 对 象 。 三 轴 视 觉 标 记 将 出 现在 对 象 上 ,这 允许 用 户 在 z+、y 和 < 方向 上 移动 对 象 。 用 
户 也 可 以 只 单 击 对 象 本 身 并 拖 动 它 在 zy 平面 上 移动 。 用 户 可 以 通过 按 住 +、y 或 x 键 的 同 
时 , 拖 动 对 象 来 控制 沿 哪个 轴 移 动物 体 。 用 户 可 以 按 住 Ctrl 键 控制 对 象 在 1 米 网 格 上 移 
动 。 如 果 对 象 未 与 场景 对 齐 (例如 ,在 使 用 下 面 介绍 的 旋转 工具 之 后 ), 用 户 可 以 按 住 Shift 
键 ,使 视觉 标记 显示 与 场景 对 齐 ,并 且 用 户 可 以 在 场景 坐标 中 进行 平移 。 

7. 旋转 

旋转 工具 允许 用 户 重新 改变 模型 +、y 和 < 轴 朝 向 。 现 在 选择 此 工具 (或 按 R 键 ) ,然后 
单 击 要 移动 的 对 象 。 三 个 环形 可 视 标 记 将 出 现在 对 象 上 ,这 允许 用 户 围绕 zx、y 和 < 轴 旋 转 
对 象 。 用 户 也 可 以 直接 单 击 对 象 本 身 并 按 住 +、y 或 = 键 ,同时 拖 动 它 从 而 限制 它 在 这 些 轴 
中 的 一 个 上 移动 。 用 户 也 可 以 按 住 Ctrl 键 以 45 度 增 量 移动 。 如 果 对 象 未 与 场景 对 齐 , 则 
可 以 按 住 Shift 键 ,以 使 可 视 标 记 显示 与 场景 对 齐 ,并 且 对 象 可 以 围绕 场景 轴 旋 转 。 

8. 缩放 

缩放 工具 允许 用 户 在 xz、y 和 < 方向 调整 模型 的 大 小 。 目 前 ,缩放 工具 仅 适 用 于 简单 的 
形状 , 即 立方 体 、 圆 柱 形 和 球形 。 选 择 此 工具 (或 按 S 键 ) ,然后 单 击 一 个 简单 的 形状 。 三 轴 
视觉 标记 将 出 现在 对 象 上 ,这 允许 用 户 缩放 对 象 的 +、y 和 < 维度 。 用 户 也 可 以 直接 单 击 对 
SU HERE ry 或 = 键 .同时 拖 动 它 从 而 将 缩放 限制 在 这 些 轴 中 的 一 个 。 用 户 还 可 以 按 
住 Ctrl 键 以 1 米 为 增 量 缩放 。 
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9. 删除 模型 

可 以 通过 选择 模型 并 单 击 Delete 键 .或 通过 右键 单 击 模型 并 选择 Delete 来 删除 模型 。 
请 尝试 删除 几 个 模型 。 

10. 保存 场景 

一 旦 用 户 对 一 个 场景 很 满意 , 它 可 以 通过 File 菜单 保存 。 选 择 File 菜单 ,然后 选择 
Save World As, 将 出 现 一 个 弹出 窗口 ,要 求 用 户 输入 新 的 文件 名 。 输 入 my_world. sdf 并 单 
击 OK 按钮 。 

11. 载 入 场景 

可 以 在 命令 行 中 加 载 保存 的 场景 ,输入 以 下 命令 : 


gazebomy world. sdf 


文件 名 必须 位 于 当前 工作 目录 中 ,或 者 必须 指定 完整 路 径 。 


4.5.2 修改 场景 
本 部 分 内 容 介 绍 如 何 修改 全 局 属性 ,包括 场景 和 物理 属性 。 
1. 场景 属性 


打开 Gazebo, 在 World 选项 卡 中 ,选择 scene 项 日 ,如 图 4-35 所 示 。 场景 属性 列表 将 显 
示 在 下 面 的 列表 框 中 。 单 击 三 角形 以 展开 属性 。 这 些 属 性 允许 用 户 更 改 环境 光 。 注 意 : 如 
果 启 用 天 空 (Sky) ,背景 颜色 不 会 改变 。 

2. 物理 属性 

在 World 选项 卡 中 ,选择 physics 项 目 。 物 理 属性 列表 将 显示 在 下 面 的 列表 框 中 。 

enable physics 复 选 框 ,可 以 用 来 禁用 物理 引擎 同时 允许 插件 和 传感器 继续 运行 。 

real time update rate 参数 以 Hz 为 单位 指定 每 秒 尝试 的 物理 更 新 数 。 如 果 此 数字 设置 
为 零 , 它 将 尽 可 能 快运 行 。 注 意 ,real time update rate 和 的 max step size 乘积 表示 目标 real 
time factor, 或 仿真 时 间 与 实时 的 比率 。 

max step size 指定 了 在 每 个 物理 更 新 步骤 的 持续 时 间 。 

在 重力 板块 : zx,y 和 < 参数 以 m/s^ 2 设置 全 局 重力 矢量 分 量 。 

在 求解 器 板块 中 : 

(1) iterations 参数 指定 用 于 迭代 LCP 求解 器 (由 ODE 和 项 目 符号 使 用 ) 的 迭代 次 数 。 

(2) SOR 参数 代表 连续 的 过 松弛 ,可 以 用 于 尝试 加 速 迭 代 方 法 的 收敛 。 

约束 块 包含 与 求解 约束 相关 的 几 个 参数 : CFM 与 ERP 参数 分 别 代 表 约 束 力 混合 和 减 
少 错误 参数 ,它们 被 ODE 和 bullet 使 用 。CFM 和 ERP 参数 可 以 与 线性 刚度 和 阻尼 系数 相 
关 。max velocity 和 surface layer 参数 被 用 来 解决 分 流 脉 冲 法 的 接触 。 任 何 穿 透 深度 超过 
由 surface layer 指定 的 深度 并 具有 小 于 max velocity 的 法 向 速度 的 接触 将 不 会 弹 回 。 

在 SDF 的 物理 文件 可 以 看 到 对 这 些 参 数 的 说 明 。 
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Gazebo : default 


图 4-35 属性 设置 对 话 框 


4.5.3 如 何在 Gazebo 中 使 用 DEM 


数字 高 程 模型 (DEM) 是 地 形 表面 的 3D 表示 ,不 包括 任何 对 象 ,如 建筑 物 或 植被 。 
DEM 通常 结合 传感器 来 创建 ,诸如 LIDAR .雷达 或 照相 机 。 对 地 面 位 置 的 地 形 海拔 以 规则 
的 水 平 间隔 进行 采样 。DEM 仅仅 是 通用 面值 ,而 不 是 特定 格式 。 事实 上 ,DEM 可 以 表示 
为 海拔 网 格 ( 光 栅 ) 或 基于 矢量 的 三 角形 不 规则 网 络 (TIN)。 目 前 .Gazebo 仅 支持 GDAL 中 
支持 的 格式 的 栅 格 数据 。 

在 Gazebo 中 支持 DEM 的 主要 动机 是 能 够 仿真 逼真 的 地 形 。 对 于 救援 或 农业 应 用 方 
面 感 兴趣 的 用 户 可 以 使 用 与 真实 场景 匹配 的 仿真 地 形 来 测试 其 机 器 人 行为 。 

1. 令 Gazebo 支持 DEM 

为 了 使 用 DEM 文件 ,用 户 应 该 安装 GDAL 库 , 输 入 以 下 命令 : 


$ sudo apt- get install gdal- bin libgdal - dev libgdallh python- gdal 
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2. DEM 文件 和 定义 转换 为 SDF 格式 

有 几 个 组 织 提供 海拔 数据 。 例 如 ,让 我 们 下 载 在 20 世纪 80 年 代 圣 海伦 山 的 火山 喷发 
之 前 或 之 后 的 DEM 文件 。 这 些 文件 用 于 公共 领域 ,由 USGS 分 发 。 

解压 缩 文件 并 将 其 重 命名 mtsthelens. dem. 修改 内 容 如 下 : 


cd 一 /Downloads 

wget https://bitbucket. org/osrf/gazebo tutorials/raw/default/dem/files/mtsthelens before. zip 
unzip —/Downloads/mtsthelens before.zip - d /tmp 

nv /tnp/30. 1.1. 1282760. den /tmp/ntsthelens.dem 


通常 ,DEM 文件 具有 大 分 辩 率 ,Gazebo 无 法 处 理 它 , 因 此 调整 DEM 的 分 辩 率 是 个 好 
办 法 。 下 一 个 命令 将 地 形 缩 放 到 129» 129 ,并 将 它 复 制 到 Gazebo media/dem/ 目录 中 ,输入 
以 下 命令 : 


$ mkdir - p /tmp/media/dem/ 
$ gdalwarp -ts 129 129 /tmp/mtsthelens. dem /tmp/media/dem/mtsthelens 129.dem 


Gazebo 中 的 DEM 文件 以 与 加 载 高 度 图 图 像 相同 的 方式 加 载 。Gazebo 自动 检测 文件 
是 普通 图 片 还 是 DEM 文件 。 创 建文 件 volcano. world 并 复制 以 下 的 内 容 。 将 文件 保存 到 
用 户 想 要 保存 的 任何 位 置 ,例如 /tmp。 


<?xml version= "1.0" ?> 

< sdf version= "1.4"> 

< world name = "default"> 

<! — A global light source -一 > 
< include? 

€ uri» model://sun «/uri» 

X/ include» 


< model nane = "heightmap"> 
«static» true X/static > 

< link name = "link" 

«X collision name= "collision" 

< geonetry > 

< heightmap > 

<uri> file://media/dem/ntsthelens 129.dem </uri > 
< size» 150 150 50 </size> 

<pos>0 0 0 </pos > 

</heightmap > 

</geometry > 

</collision> 
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< visual name = "visual abcedf"> 

< geometry > 

< heightmap > 

< texture > 

< diffuse» file://media/materials/textures/dirt_diffusespecular. png </diffuse > 
< normal > file: //media/materials/textures/flat_normal. png </normal > 
< size> 1 </size> 

</texture> 

«texture? 

< diffuse» file://media/materials/textures/grass diffusespecular. png «/diffuse^ 
< normal > file: //media/materials/textures/flat normal. png </normal > 
<size>1</size> 

</texture> 

«texture» 

< diffuse» file://media/materials/textures/fungus diffusespecular. png </diffuse > 
< normal > file://media/materials/textures/flat normal. png </normal > 
<size>1</size> 

</texture> 

<blend> 

«nin height» 2 «/min height» 

< fade dist» 5 «/fade dist» 

«/blend» 

«blend» 

«nin height» 4 «/nin height» 

< fade dist» 5 «/fade dist» 

X/blend» 

«uri» file://media/dem/ntsthelens 129.dem </uri > 

< size> 150 150 50 </size> 

«pos» 0 0 0 «/pos» 

</heightmap > 

</geometry> 

</visual > 


</link> 
</model> 


</world> 
</sdf > 


上 面 代码 中 的 < heightmap >< size > 元 素 告诉 Gazebo 是 否 加 载 具 有 原始 维度 ( 当 < size > 
不 存在 时 ) 或 缩放 ( 当 < size > 存在 时 ) 的 DEM。 如 果 用 户 喜欢 缩放 DEM. « size > 元 素 告 诉 
Gazebo 在 仿真 中 以 米 为 单位 的 地 形 的 大 小 。 如 果 要 保持 正确 的 宽 高 比 , 请 务必 正确 计算 宽 
JE .高度 和 海拔 (这 是 < size > 中 的 第 3 个 数字 )。 在 示例 中 ,DEM 将 缩放 为 150 150m 的 正 
方形 ,最 大 海拔 为 50m。 
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启动 Gazebo 并 打开 包含 用 户 的 DEM 文件 的 场景 ,用 户 应 该 看 到 火山 。 在 示例 中 ,该 
文件 位 于 / tmp 目录 中 。 


# Be sure of sourcing gazebo setup. sh in your own installation path 
$ source /usr/share/gazebo/setup. sh 
$ GAZEBO RESOURCE PATH = " $ GAZEBO RESOURCE PATH:/tmp" gazebo /tmp/volcano. world 


3. 获得 所 需要 区 域 的 DEM 文件 

接 下 来 将 描述 一 种 用 于 获得 特定 感 兴趣 区 域 的 DEM 文件 的 方法 。 

全 球 土地 覆盖 设施 维护 着 地 球 的 高 分 辨 率 数字 地 形 数据 库 。 转 到 其 搜索 和 预览 工具 ， 
用 户 会 看 到 类 似 下 面 的 图 像 。 每 个 地 形 补丁 都 有 一 个 唯一 的 路 径 和 行 ,用 户 应 该 在 使 用 该 
工具 之 前 知道 。 我 们 将 使 用 QGIS 来 发 现 感 兴趣 区 域 的 路 径 / 行 。 

QGIS 是 一 个 跨 平台 的 开源 地 理 信息 系统 程序 ,提供 数据 查看 ,编辑 和 分 析 功 能 。 按 照 
QGIS 网 站 上 详细 说 明 下 载 QGIS。 打 开 QGIS, 单 击 左 栏 图 标 标注 WMS/ WMTS layer, 单 
击 Add default servers, 选 择 Lizardtech server, 然 后 . 按 下 connect 按钮 。 选 择 MODIS 值 并 
TET Add 按钮 。 关 闭 弹 出 窗口 。 下 一 步 是 添加 具有 所 有 不 同 补丁 的 另 一 个 层 。 下 载 此 
shapefile, 并 在 任意 文件 夹 中 解压 缩 。 回 到 QGIS 并 按 Add Vector Layer( 左 栏 图 标 ) 。 按 
Browse, 然 后 选择 以 前 未 压缩 的 wrs2 descending. shp 文件 。 在 打开 的 窗口 中 按 Open。 现 
在 ,用 户 将 在 主 窗口 中 看 到 两 个 图 层 。 改 变 wrs2 降序 层 的 透明 度 ,以 便 能 够 同时 看 到 两 个 
图 层 。 双 击 wrs2_descending 图 层 , 然 后 将 其 透明 度 值 修改 为 大 约 85%。 

使 用 滚动 和 左 按钮 导航 到 用 户 感 兴趣 的 区 域 ,然后 单 击 顶 部 栏 上 标记 的 Identify 
Features 图 标 。 单 击 用 户 感 兴趣 的 区 域 . 该 区 域 周围 的 所 有 地 形 块 将 突出 显示 。 新 的 弹出 
窗口 将 显示 每 个 突出 显示 的 补丁 的 路 径 / 行 值 。 在 下 面 的 图 片 中 ,用 户 可 以 看 到 包含 拉 斯 帕 
尔 马 斯 的 DEM 修补 程序 的 路 径 和 行 , 这 是 西班牙 加 那 利 群 岛 的 天 堂 之 一 。 

使 用 GLCF 搜索 工具 返回 到 浏览 器 ,并 在 标记 为 Start Path 和 Start Row 的 列 中 写 入 
路 径 / 行 值 。 然 后 单 击 Submit Query; 按 Preview and Download 查看 用 户 的 结果 。 选 择 用 
户 的 地 形 文件 ,然后 按 Download。 最 后 .选择 扩展 名 为 . gz 的 文件 ,并 在 用 户 喜 欢 的 文件 夹 
中 解压 缩 。 全 球 土 地 覆盖 设施 文件 是 GeoTiff 格式 ,是 最 常见 的 可 用 DEM 文件 格式 之 一 。 

4. 准备 DEM 数据 以 在 Gazebo 中 使 用 

DEM 数据 通常 以 非常 高 的 分 辩 率 创建 。 在 Gazebo 中 使 用 它 之 前 ,使 用 gdalwarp 命令 
将 地 形 分 辩 率 降低 到 更 适合 管理 的 大 小 ,输入 以 下 命令 : 


$ gdalwarp -ts < width>< height >< srcDEM>< targetDEM > 


DEM 数据 通常 包含 "holes” 或 void" 区域 。 这 些 部 分 对 应 于 在 创建 DEM 时 无 法 收集 
数据 的 区 域 。 在 数据 为 hole” 的 情况 下 ,将 为 该 孔 分 配 该 DEM 中 使 用 的 数据 类 型 的 最 小 或 
最 大 值 。 

始终 尝试 下 载 已 完成 的 DEM 数据 集 的 "完成 "版 本 ,这 其 中 * 孔 "已 经 被 填充 。 如 果 用 
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户 的 DEM 地 形 包含 孔 (也 称 为 NODATA 值 ), 请 尝试 使 用 gdal 工具 手动 修复 它 ,例如 


gdal fillnodata.py 


5. 在 Gazebo 中 使 用 多 个 DEM 

虽然 Gazebo 不 直接 支持 多 个 DEM, 但 GDAL 有 一 套用 于 将 一 组 DEM 合并 成 一 个 
DEM 的 实用 程序 。 第 一 步 是 下 载 要 合并 的 DEM EA. EE ATEEN IRER: 
GDAL 将 无 颖 地 合并 它们 。 假 设 当前 目录 包含 一 组 可 以 合并 的 Geotiff 文件 ,输入 以 下 
LE 


$ gdal merge.py * .tif -o dem merged.tif 


现在 ,在 场景 文件 中 ,你 只 能 使 用 dem. merged. tif 场景 ,Gazebo 将 加 载 地形 与 所 有 的 
补丁 合并 。 如 图 4-36 Bros ,用 户 可 以 看 到 合并 加 那 利 群 岛 周围 四 个 地 形 补丁 的 结果 。 


图 4-36 Gazebo 加 载 得 到 的 地 形 


4.5.4 模型 群 


本 部 分 内 容 演 示 了 如 何 使 用 SDF < population > 标记 创建 模型 群 。 模 型 总 体 由 相同 模 
型 的 集合 组 成 。 添 加 模型 总 体 是 指定 以 下 参数 的 问题 : 

D 模型 (例如 : table,coke_can) 。 

© 作为 总 体 一 部 分 的 对 象 数 。 

O 其 中 将 布置 对 象 的 容器 的 形状 和 尺寸 (例如 box,cylinder) 。 

CD. 群体 容器 的 位 置 和 方向 。 
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C 容器 内 对 象 的 分 布 (例如 : random,grid) 。 

作为 参考 ,请 检查 SDF 规范 中 < population > 标记 及 其 参数 的 完整 规范 。 
1. 创建 对 象 群 

1) 快速 开始 

(1) 从 为 本 部 分 内 容 创 建 一 个 目录 开始 ,输入 以 下 命令 : 


(2) 下 载 此 文件 : can. population. world 进入 当前 目录 。 输 入 以 下 命令 : 


用 户 应 该 得 到 这 个 场景 文件 , 它 的 内 容 为 : 
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< type > random </type > 
</distribution> 
</population> 


</world> 
</sdf > 


(3) 打开 Gazebo, 输 入 以 下 命令 : 


gazebocan population. world 


HP MZAA — 4 2j T f (17 09 TE F8] £6 9 S I1 dE 3 Bl DLE R a o a pot EER ST D 
2X2X0. 01 米 的 箱 式 容器 中 。 

2) 场景 解释 

进一步 了 解 can. population. world 的 不 同 元 素 , 它 的 部 分 内 容 如 下 : 


< population name = "can_populationl"> 
< model nane = "canl"» 

< include? 

< static» true </static > 

X uri» model://coke can </uri > 

x/ include» 

«/model > 


在 这 个 片段 中 ,可 以 看 到 如 何 通过 使 用 < population > 标签 来 指定 一 个 总 体 元 素 。 每 个 
群体 应 该 有 一 个 唯一 的 名 称 , 这 是 由 name 属性 指定 的 。 在 population 标记 中 ,用 户 可 以 看 
到 如 何 使 用 < model > 标记 来 选择 模型 。 群 体 的 每 个 元 素 在 仿真 中 将 具有 唯一 名 称 地 插 人 
仿真 ,该 唯一 名 称 将 通过 以 后 缀 _clone_i 附加 到 模型 名 称 来 创建 ,其 中 i 是 群体 的 第 i 个 元 
Ko WME 4-37 所 示 ,用 户 可 以 看 到 在 Gazebo 场景 中 产生 的 模型 列表 。 

最 常见 的 群体 类 型 包括 无 生命 物体 ,如 树木 .岩石 和 建筑 物 。 建 议 用 户 将 < population > 
标记 用 于 静态 模型 ,并 排除 移动 实体 (例如 机 器 人 ) ,这 些 移动 实体 通常 需要 更 精确 的 展示 位 
置 ,数量 较 少 。 

<pose>000000</pose> 

<box> 


<size>220.01</size> 
</box> 


上 面 的 代码 块 指定 了 对 象 将 被 放置 的 区 域 。 在 这 种 情况 下 ,所 有 对 象 都 在 具有 2X2X 
0.01m 以 (0,0,0) 为 中 心 ,具有 朝向 (0.0,0) 的 边 的 3D 边界 框 内 产生 。 作 为 < box > 的 替代 ， 
通过 指定 其 半径 和 长 度 的 < cylinder > 区 域 也 是 允许 的 。( 检 查 SDF 规范 以 查看 
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图 4-37 Gazebo 场景 中 产生 的 模型 列表 
< cylinder > 的 参数 的 完整 描述 。)< pose > 元 素 设置 总 体 区 域 的 参考 框架 。 


«model count>10</model count» 


上 面 用 户 可 以 看 到 入 口中 的 模型 数量 是 如 何 确定 的 。 任 何 正 数 都 是 允许 的 ,但 需 考 虑 
数字 越 高 ,性 能 可 能 受到 的 影响 就 越 大 。 


< distribution> 
<type > random </type> 
</distribution> 


< distribution > 元 素 设 置 对 象 是 如 何 放置 的 区 域内 。 

3) 分 布 类 型 

random: 随机 放置 的 模型 。 请 注意 ,对象 可 能 会 彼此 冲突 。 

uniform; 模型 放置 在 伪 2D 网 格 模式 。 使 用 K-Means 近似 解决 方案 ,并 找到 区 域内 指 
定 对 象 的 数量 。 

grid: 模型 均匀 放置 在 2D 网 格 图 案 中 。 此 分 布 还 需要 指定 行 数 , 列 数 和 每 个 元 素 之 间 
的 距离 。 请 注意 ,元 素 < model count > 在 此 分 布 中 将 被 忽略 。 插 入 到 仿真 中 的 对 象 数量 将 
等 于 行 数 乘 以 列 数 。 

linear-x: 模型 沿 着 全 局 工 轴 均 匀 放 置 为 一 行 。 

linear-y: 模型 沿 着 全 局 y 轴 均 匀 放 置 在 一 行 。 

linear-z: 模型 沿 着 全 局 = 轴 均 匀 放 置 在 一 行 。 

对 于 更 高 级 的 示例 ,用 户 可 以 检查 使 用 Gazebo 部 署 的 population. world 场景 文件 。 
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当然 ,用 户 可 以 进行 测试 ,输入 以 下 命令 : 


gazebo worlds/population. world 


4. 


5.5 建筑 编辑 器 


本 部 分 内 容 介绍 使 用 构建 编辑 器 创建 建筑 物 的 过 程 。 
启动 Gazebo, 在 Edit 菜单 上 。 如 图 4-38 所 示 , 转 到 Building Editor 或 单 击 Ctrl 十 B 组 
合 键 打开 编辑 器 。 


Crete walls. 


Add Features 


图 4-38 ”建筑 编辑 器 的 编辑 界面 


接 下 来 进入 图 形 用 户 界面 。 如 图 4-39 所 示 ,编辑 器 由 以 下 3 个 方面 组 成 : 

(1) 调 色 板 ,在 这 里 用 户 可 以 选择 建筑 的 特征 和 材料 。 

(2) 在 2D 视图 中 ,用 户 可 以 导入 平面 图 以 跟踪 (可 选 ), 并 插入 墙壁 ,窗户 、 门 和 楼 梯 。 

(3) 在 3D 视图 中 ,用 户 可 以 看 到 用 户 的 建筑 的 预览 。 用 户 可 以 在 这 里 给 用 户 的 建筑 物 
的 不 同 部 分 分 配 颜色 和 纹理 。 


T: 


用 


导入 平面 图 
户 可 以 从 头 开始 创建 场景 ,也 可 以 使 用 现 有 图 像 作为 模板 进行 跟踪 。 例 如 ,该 图 像 可 


以 是 建筑 物 的 2D 激光 扫描 。 
如 图 4-40 所 示 , 单 击 这 里 获得 平面 图 示例 ,然后 执行 如 下 操作 : 
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palette 


图 4-39 图 形 用 户 界面 编辑 器 


图 4-40 导入 平面 图 
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CD 单 击 Import 按钮 。 将 出 现 Import Image 对 话 框 。 

(2) 步骤 1: 选择 用 户 以 前 保存 在 计算 机 上 的 图 像 , 然 后 单 击 Next. 

(3) 步骤 2: 要 确保 用 户 在 图 像 上 跟踪 的 墙壁 以 正确 的 比例 显示 ,用 户 必须 以 每 像素 
(px/m) 为 单位 设置 图 像 的 分 辨 率 。 如 果 我 们 知道 分 辨 率 ,可 以 直接 在 对 话 框 中 键入 ,然后 
单 击 OK。 在 这 个 例子 中 ,我 们 不 知道 分 辩 率 ,但 知道 图 像 中 两 点 之 间 的 真实 场景 距离 ( 例 
如 ,7.5 米 的 顶 壁 ) ,因此 可 以 使 用 它 来 计算 分 辩 率 : 

O 单 击 / 释 放 墙 的 一 端 。 当 用 户 移动 鼠标 时 ,会 出 现 一 条 橙色 线 。 

@ 在 墙 的 结尾 单 击 /释放 以 完成 线 。 

© 现在 在 对 话 框 中 键入 距离 (以 米 为 单位 )( 在 这 种 情况 下 为 7. 5 米 )。 分 辨 率 将 根据 
用 户 绘制 的 线 自动 计算 。 

© 然后 可 以 单 击 OK 按钮 。 

(4) 图 像 将 显示 在 适当 缩放 的 2D 视图 上 。 

注意 : 添加 更 多 楼 层 后 ,用 户 可 以 通过 重复 相同 的 过 程 为 每 个 楼 层 计 划 导 入 平面 图 。 

2. 添加 功能 

1) 添加 墙壁 

跟踪 平面 图 上 的 所 有 墙壁 。 记 住 ,我 们 将 在 后 面 的 墙 上 安装 门窗 ,所 以 在 这 里 用 户 可 以 
在 他 们 的 墙 上 画 。 不 要 担心 太 多 ,如 果 墙 壁 不 完美 ,可 以 在 以 后 编辑 它们 。 

CD 在 调 色 板 上 , 单 击 Wall。 

(2) 在 2D 视图 上 , 单 击 /释放 任何 位 置 以 启动 墙 。 移 动 鼠 标 时 ,会 显示 墙壁 的 长 度 。 

(3) 再 次 单 击 以 结束 当前 墙 并 启动 相 邻 的 墙 。 

(4) 双击 以 完成 墙 ,而 不 画 新 墙 。 

注意 : 用 户 可 以 右键 单 击 或 按 Esc 取消 绘制 当前 墙壁 段 ; 默认 情况 下 ,墙壁 snap( 捕 
310159 0. 25m 的 增 量 和 现 有 墙壁 的 终点 ; 要 覆盖 此 功能 ,请 在 绘图 时 按 住 Shift。 

2) 添加 门窗 

如 图 4-41 所 示 ,在 平面 图 上 显示 的 位 置 插入 门窗 。 

CD 在 调 色 板 上 , 单 击 Window 或 Door。 

(2) 当 用 户 在 2D 视图 中 移动 鼠标 时 ,要 插 和 人 的 要 素 将 随 之 移动 ,与 3D 视图 中 的 对 应 
物 一 样 。 

注意 : 当 用 户 将 鼠标 是 停 在 窗户 上 时 ,窗口 和 门 会 自动 捕 提 到 墙壁 。 移 动 时 显示 墙壁 
两 端的 距离 。 

G) 单 击 所 需 的 位 置 以 放置 要 素 。 

注意 : 在 墙壁 上 绘制 墙 后 ,可 能 很 难看 到 用 户 的 楼 面 平面 图 上 的 特征 。 为 了 使 它 更 容 
易 , 在 2D 视图 的 顶部 ,用 户 可 以 选择 查看 或 隐藏 平面 图 或 当前 楼 层 的 特征 。 用 户 还 可 以 使 
用 热 键 切换 可 见 性 ,F 表示 平面 图 ,G 表示 特征 。 

3) 添加 楼 梯 

如 图 4-42 所 示 ,在 这 个 平面 图 上 没有 楼 梯 ,但 我 们 将 插入 一 个 。 
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图 4-41 添加 门窗 后 的 地 图 


图 4-42 添加 了 楼 梯 的 地 图 


COD 在 调 色 板 上 , 单 击 Stairs, 
(2) 当 用 户 在 2D 视图 中 移动 鼠标 时 ,要 插入 的 楼 梯 将 随 之 移动 ,与 3D 视图 中 的 对 应 
物 一 样 。 
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(3) 选择 用 户 的 楼 梯 的 位 置 ,然后 单 击 放 置 它 。 

4) 添加 楼 层 

我 们 已 经 完成 了 楼 层 1。 让 我 们 在 建筑 中 添加 另 一 层 ,这 样 就 完成 了 楼 梯 模 型 的 搭建 。 
如 图 4-43 所 示 ,在 2D 视图 的 顶部 , 单 击 “ 十 "号 以 添加 楼 层 。 或 者 ,右键 单 击 2D 视图 并 选 
TÉ Add a level。 添 加 新 楼 层 时 ,会 自动 插入 地 板 。 如 果 在 楼 层 的 下 面 有 楼 梯 , 楼 梯 上 方 的 一 
个 洞 将 在 建筑 物 被 保存 时 从 地 板 上 切 掉 。 


图 4-43 添加 了 楼 层 的 地 图 


注意 : 目前 ,所 有 楼 层 都 是 矩形 ; 在 添加 楼 层 之 前 ,请 确保 用 户 当前 楼 层 上 有 墙 , 以 便 
在 其 上 创建 ; 目前 ,下 面 的 楼 层 的 所 有 墙壁 都 使 用 默认 材料 被 复制 到 新 的 楼 层 , 不 复制 其 他 
功能 ,用 户 可 以 手动 删除 不 想 要 的 墙 。 

3. 编辑 用 户 的 建筑 物 

注意 : 编辑 建筑 物 时 要 小 心 ;编辑 器 目前 无 法 撤销 用 户 的 操作 ; 所 有 测量 均 以 米 为 
单位 。 

1) 更 改 楼 层 

由 于 我 们 添加 了 一 个 楼 层 ,在 2D 视图 中 达到 了 新 的 水 平面 。 用 户 可 以 通过 从 2D 视图 
顶部 的 下 拉 列 表 中 选择 它 来 返回 到 楼 层 1。 需 要 注意 的 是 ,在 2D 视图 中 当前 选择 的 楼 层 将 
在 3D 视图 中 显示 为 半 透 明 ,并且 其 下 方 的 所 有 楼 层 将 显示 为 不 透明 。 上 面 的 楼 层 将 被 隐 
藏 。 需 要 记 住 的 是 ,它们 仍然 是 用 户 的 建筑 的 一 部 分 。 

此 外 ,也 可 以 编辑 一 些 楼 层 配 置 : 双击 2D 视图 以 打开 具有 楼 层 配 置 选项 的 检查 器 。 或 
者 ,右键 单 击 并 选择 Open Level Inspector. 
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用 户 可 能 已 经 添加 了 用 户 不 想 要 的 楼 层 , 或 者 可 能 觉得 当前 楼 层 混乱 ,并 希望 重新 开始 
。 要 删除 当前 楼 层 ,请 按 2D 视图 顶部 的 -按钮 ,或 右键 单 击 并 选择 Delete Level, 

2) 编辑 墙 

在 2D 视 图 中 , 单 击 要 编辑 的 墙壁 。 

CL) 通过 将 墙 面 拖 动 到 新 位 置 来 平移 墙 面 。 

(2) 通过 拖 动 其 中 一 个 端点 来 调整 墙 的 大 小 或 旋转 墙 。 

注意 : 

A) 默认 情况 下 ,墙壁 以 15" 和 0.25m 为 增 量 snap。 要 和 覆盖 此 功能 ,请 在 绘图 时 按 住 
Shift。 双 击 2D 视 图 中 的 墙 以 打开 具有 配置 选项 的 检查 器 。 或 者 ,右键 单 击 并 选择 Open 
Wall Inspector。 编 辑 某 些 字段 ,然后 按 Apply 以 预览 更 改 。 

要 删除 墙 ,请 在 选中 需要 删除 的 墙 时 按 Delete 键 ,或 右键 单 击 2D 视图 中 的 墙 并 选择 Delete, 

(2) 编辑 墙 会 考虑 附加 的 墙 。 

(3) 删除 墙 会 删除 所 有 连接 到 它 的 门 和 窗户 。 

3) 编辑 门窗 

正如 在 编辑 墙壁 时 做 的 ,我 们 可 以 以 几 种 不 同 的 方式 更 精确 地 操纵 门窗 。 如 图 4-44 所 
示 , 在 2D 视图 中 , 单 击 要 编辑 的 要 素 。 


nt 


图 4-44 门窗 编辑 界面 
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CD 通过 将 要 素 拖 动 到 新 位 置 来 平移 它 。 记 住 , 门 和 门 自动 地 卡 在 墙 上 ,把 它们 从 任何 
墙 上 分 离 没 有 任何 意义 ,因为 它们 代表 在 墙 上 的 洞 。 

(2) 通过 拖 动 旋转 手柄 来 旋转 要 素 。 目 前 ,只 要 它们 在 墙 上 ,它们 的 方向 就 没有 什么 
区 别 。 

O 通过 拖 动 其 中 一 个 端点 来 调整 要 素 的 宽度 。 双 击 2D 视图 中 的 要 素 以 打开 带 有 配 
置 选项 的 检查 器 。 或 者 ,右键 单 击 并 选择 Open Window/Door Inspector。 要 删除 特征 ,在 
选中 时 按 下 Delete 键 ,或 在 2D 视图 中 右键 单 击 它 ,然后 选择 Delete. 

4) 编辑 楼 梯 

最 后 ,编辑 我 们 之 前 插入 的 楼 梯 。 由 于 它 不 是 在 平面 图 上 ,我 们 可 以 根据 所 想 有 些 创 意 
和 调整 它 的 大 小 。 如 图 4-45 所 示 ,在 2D 视图 中 , 单 击 楼 梯 选 择 它 。 


图 4-45 ”楼梯 编辑 界面 


CD 通过 将 楼 梯 拖 动 到 新 位 置 来 平移 楼 梯 。 

(2) 通过 拖 动 其 旋转 手柄 以 90 的 倍数 旋转 楼 梯 。 

(3) 通过 拖 动 其 中 一 个 端 节点 来 调整 楼 梯 的 大 小 。 双 击 2D 视图 中 的 楼 梯 ,打开 带 有 配 
置 选项 的 检查 器 。 或 者 ,右键 单 击 并 选择 Open Stairs Inspector。 要 删除 楼 梯 , 请 在 Delete 
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选择 时 按键 ,或 右键 单 击 并 选择 Delete。 需 要 注意 的 是 ,在 2D 视图 中 ,楼 梯 在 开始 和 结束 
楼 层 都 可 见 。 

5) 添加 颜色 和 纹理 

现在 ,一 切 都 正确 放置 并 具有 大 小 ,用 户 可 以 分 配 墙壁 ,地 板 和 楼 梯 的 颜色 和 纹理 。 记 
住 ,窗户 和 门 只 是 墙 上 的 洞 ,因此 不 能 有 材料 。 其 中 ,默认 颜色 为 白色 ,默认 纹理 为 无 。 

6) 通过 检查 器 

如 图 4-46 所 示 , 用 户 可 以 分 别 从 Wall Inspector、Stairs Inspector 和 Level Inspector 给 
墙壁 ,楼 梯 和 地 板 添 加 颜色 和 质地 。 只 需 打开 检查 器 ,选择 用 户 的 材料 ,然后 按 Apply. 


图 4-46 ”检查 器 设置 对 话 框 


7) 通过 调 色 板 

可 以 从 调 色 板 中 选择 颜色 和 纹理 ,并 通过 在 3D 视图 中 单 击 它们 将 其 分 配给 建筑 物 上 
的 项 目 。 

COD 在 调 色 板 中 单 击 颜色 或 纹理 。 

(2) 当 用 户 在 3D 视图 中 移动 鼠标 时 , 悬 停 选 中 的 要 素 将 高 亮 显示 ,显示 所 选材 料 的 
预览 。 

(3) 单 击 高 亮 的 特征 会 为 其 分 配 所 选材 料 。 用 户 可 以 根据 需求 任意 选择 特征 。 

(4) 完成 所 选材 质 后 ,右键 单 击 3D 视图 .或 单 击 选中 要 素 外 的 任何 部 位 以 离开 材质 
模式 。 

Gazebo 5. 1 中 的 新 功能 : 要 选择 自 定义 颜色 ,在 调 色 板 上 单 击 More。 如 图 4-47 所 示 ， 
将 打开 一 个 对 话 框 ,用 户 可 在 其 中 指定 自 定义 颜色 。 

注意 : 每 个 地 图 项 只 能 有 一 种 颜色 和 一 个 纹理 。 将 相同 的 材料 分 配给 要 素 的 所 有 面 ; 
目前 ,不 能 在 建筑 编辑 器 上 分 配 自 定义 纹理 。 
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图 4-47 调 色 板 设置 对 话 框 


4. 保存 用 户 的 建筑 物 

保存 将 为 用 户 的 建筑 物 创 建 一 个 目录 SDF 和 配置 文件 。 如 图 4-48 Bros ,保存 之 前 ,请 
在 调 色 板 上 为 用 户 的 建筑 命名 。 

在 顶部 菜单 上 ,选择 File 菜单 .然后 单 击 Save As 按钮 (或 按 下 Ctrl 十 S 键 )。 如 图 4-49 
所 示 ,将 出 现 一 个 对 话 框 , 用 户 可 以 在 其 中 选择 模型 的 位 置 。 需 要 注意 的 是 ,在 Advanced 
Options 选项 下 用 户 可 以 为 用 户 的 建筑 设置 一 些 元 数据 。 


Save Model 


Building Editor ? 


Create Walls 


图 4-48 建筑 命名 对 话 框 图 4-49 保存 模型 对 话 框 
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5. 退出 

当 用 户 完成 创建 建筑 物 并 保存 之 后 ,请 转 到 File 菜单 ,然后 选择 Exit Building Editor 
命令 。 需要 注意 的 是 ,退出 建筑 编辑 器 后 ,用 户 的 建筑 物 将 不 再 可 编辑 。 如 图 4-50 所 示 , 用 
户 的 建筑 将 显示 在 主 窗口 中 。 将 来 ,用 户 可 以 在 Insert 标签 中 找到 该 建筑 。 


图 4-50 ”完成 编辑 后 的 建筑 


4.6 插件 的 编写 


插件 允许 用 户 控 制 模型 ,传感器 ,场景 (world) 属 性 ,甚至 Gazebo 运行 的 方式 。 下 面 介 
绍 如 何 为 各 种 目的 创建 和 加 载 插件 。 

插件 是 一 段 代 码 , 它 被 编译 为 共享 库 , 并 插入 到 仿真 中 。 插 件 可 以 通过 标准 的 C++ 类 直 
接 访 问 Gazebo 的 所 有 功能 。 

Gazebo 中 的 插件 的 功能 有 : 

(1) 让 开发 人 员 几 乎 控制 Gazebo 的 任何 方面 ; 

(2) 是 易于 共享 的 独立 例 程 ; 

(3) 可 以 从 正在 运行 的 系统 中 插入 和 删除 ; 


454 4| 机 器 人 仿真 与 编程 技术 


(4) 以 前 的 Gazebo 版 本 使 用 控制 器 。 

它们 表现 方式 与 插件 大 致 相同 ,但 是 被 静态 编译 为 Gazebo。 插 件 更 加 灵活 ,允许 用 户 
挑选 和 选择 仿真 中 包含 的 功能 。 

Gazebo 中 的 插件 的 使 用 情景 有 : 

CO 用 户 希 望 以 编程 方式 更 改 仿 真 。 例 如 : 移动 模型 ,响应 事件 ,给 定 一 组 前 提 条 件 插 
入 新 模型 。 

(2) 用 户 想 要 Gazebo 的 一 个 快速 接口 ,没有 传输 层 的 开销 。 例 如 : 没有 消息 的 序列 化 
和 反 序 列 化 。 

(3) 用 户 有 一 些 代 码 可 以 使 他 人 受益 ,并 想 分 享 它 。 

目前 ,Gazebo 中 的 插件 有 6 种 类 型 : 场景 ,模型 ,传感器 ,系统 ,视觉 和 GUI。 每 个 插件 
类 型 由 Gazebo 的 不 同 组 件 管理 。 例 如 ,一 个 Model 插件 被 附加 到 模型 中 ,并 控制 Gazebo 
中 特定 的 模型 。 同 样 ,一 个 World 插件 被 附加 到 一 个 场景 ,一 个 Sensor 插件 被 连接 到 一 个 
特定 的 传感器 。 系 统 插件 被 指定 在 命令 行 上 ,并 在 Gazebo 启动 期 间 首先 加 载 。 这 个 插件 
使 得 用 户 控 制 启动 过 程 。 

应 根据 所 需 的 功能 选择 插件 类 型 。 使 用 World 插件 来 控制 场景 属性 ,例如 物理 引擎 、 
环境 照明 等 。 使 用 Model 插件 来 控制 关节 和 模型 的 状态 。 使 用 传感器 插件 获取 传感器 信 
息 并 控制 传感器 属性 。 


4.6.1 一 个 简单 的 插件 : Hello WorldPlugin! 
Gazebo 中 的 插件 设计 简单 ,其 中 一 个 场景 插件 包含 一 个 有 几 个 成 员 函 数 的 类 。 首 先 ， 
如 果 用 户 从 debians 上 安装 Gazebo, 确 保 用 户 已 经 安装 了 Gazebo 开发 文件 。 如 果 从 源 端 安 


装 Gazebo, 用 户 可 以 忽略 此 步骤 。 如 果 用 户 有 一 个 除了 gazebo6 之 外 的 发 行 版 ,把 6 换 成 
用 户 有 的 版 本 号 ,输入 以 下 命令 : 


sudo apt - get install libgazebo6 - dev 


接 下 来 ,为 新 插件 创建 一 个 目录 和 一 个 .ec 文件 ,输入 以 下 内 容 ， 


$ mkdir —/gazebo plugin tutorial 
$ cd —/gazebo plugin tutorial 
$ gedit hello world.cc 


将 以 下 内 容 复 制 到 hello world. cc 中 .输入 以 下 内 容 : 


# include < gazebo/gazebo. hh» 
namespace gazebo 
ü 
class WorldPluginTutorial : public WorldPlugin 
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public: WorldPluginTutorial() : WorldPlugin() 
t 
printf("Hello World!Wn"); 
H 


public: void Load(physics::WorldPtr world, sdf::ElementPtr sdf) 
t 
} 
}; 
GZ REGISTER WORLD PLUGIN(WorldPluginTutorial) 


) 


上 面 的 代码 也 位 于 Gazebo 源码 : examples/plugins/hello: world/hello. world. cc, 以 及 
一 个 合适 的 CMakeLists. txt 文件 。 
要 编译 上 面 的 插件 ,创建 一 /gazebo_plugin_tutorial/CMakeLists. txt, 输 入 以 下 命令 : 


$ gedit ~/gazebo plugin tutorial/CMakeLists. txt 
在 CMakeLists. txt 中 复制 以 下 内 容 : 

cmake minimum required(VERSION 2.8 FATAL ERROR) 
find package(gazebo REQUIRED) 

include directories( $ (GAZEBO INCLUDE DIRS}) 
link directories( $ (GAZEBO LIBRARY DIRS]) 


list(APPEND CMAKE CXX FLAGS " $ (GAZEBO CXX FLAGS]") 


add library(hello world SHARED hello world.cc) 
target link libraries(hello world $ (GAZEBO LIBRARIES]) 


gazebo6 新 增 功能 : 现在 ,所 有 下 游 软件 都 需要 使 用 C++11 标志 来 编译 Gazebo。 这 是 
通过 以 下 行 来 完成 的 : 


Set(CMAKE CXX FLAGS " $ {CMAKE CXX FLAGS) $ (GAZEBO CXX FLAGS)") 


创建 构建 目录 ,输入 以 下 命令 : 


$ mkdir —/gazebo plugin tutorial/build 
$ cd —/gazebo plugin tutorial/build 


编译 代码 ,输入 以 下 命令 : 
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$ cmake ../ 
$ make 


编译 将 带 来 共享 库 ， 一 /gazebo_plugin_tutorial/build/libhello_world. so, 它 可 以 插入 
到 Gazebo 中 。 
最 后 ,将 库 路 径 添加 到 GAZEBO_PLUGIN_PATH, 输 入 以 下 命令 ; 


$ export GAZEBO PLUGIN PRTH = $ (GAZEBO PLUGIN PRTH] :一 /gazebo_plugin tutorial/build 


注意 ; 这 将 更 改 当前 shell 的 路 径 。 如 果 用 户 想 为 用 户 打 开 的 每 一 个 新 的 temrinal 使 
用 用 户 的 插件 ,将 上 面 的 行 追加 到 一 /. bashrc 文件 中 。 


4.6.2 插件 的 使 用 


一 旦 用 户 有 一 个 插件 编译 为 共享 库 ( 见 上 文 ), 用 户 可 以 在 SDF 文件 中 将 它 附加 到 一 个 
场景 或 模型 (更 多 信息 参见 SDF 文档 )。 在 启动 时 ,Gazebo 解析 SDF 文件 ,定位 插件 ,并 加 
载 代码 。 重 要 的 是 Gazebo 能 够 找到 插件 。 指 定 插件 的 完整 路 径 , 或 者 插件 存在 于 
GAZEBO_PLUGIN_PATH 环境 变量 中 的 一 个 路 径 中 。 

创建 一 个 场景 文件 ,并 将 下 面 的 代码 复制 到 其 中 。 示 例 场 景 文件 也 可 以 在 examples/ 
plugins/hello world/hello. world 中 找到 。 


$ gedit —/gazebo plugin tutorial/hello. world 
<?xml version = "1.0"? 
< sdf version- "1. 4"> 
< world name = "default"> 
< plugin name = "hello world" filename = "libhello world. so"/> 
</world> 
</sdf > 


现在 用 gzserver 打开 它 .输入 以 下 命令 : 


$ gzserver ~/gazebo plugin tutorial/hello.world —- verbose 


用 户 应 该 看 到 类 似 的 输出 : 


Gazebo multi- robot simulator, version 6.1.0 

Copyright (C) 2012 - 2015 Open Source Robotics Foundation. 
Released under the Apache 2 License. 
http://gazebosim.org 


[Msg] Waiting for master. 
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[Msg] Connected to gazebo master @ http://127.0.0.1:11345 
[Msg] Publicized address: 172.23.1.52 
Hello World! 


4.6.3 模型 插件 


1. 代码 
插件 允许 完全 访问 模型 及 其 基础 元 素 ( 连 杆 ,关节 ,碰撞 对 象 ) 的 物理 属性 。 以 下 插件 将 
线性 速度 应 用 到 它 的 父 模型 ,输入 以 下 命令 : 


$ cd ~/gazebo plugin tutorial 
$ gedit model push.cc 


插件 代码 为 : 


# include <boost/bind. hpp > 

# include < gazebo/gazebo. hh> 

# include < gazebo/physics/physics. hh> 
# include < gazebo/common/common. hh > 

# include < stdio. h> 


namespace gazebo 
{ 
class ModelPush : public ModelPlugin 
( 
public: void Load(physics::ModelPtr parent, sdf::ElementPtr / * _sdf /) 
t 
//Store the pointer to the model 
this -> model = parent; 


//Listen to the update event. This event is broadcast every 

//simulation iteration. 

this - > updateConnection = event: : Events: :ConnectiorldUpdateBegin( 
boost::bind(&ModelPush::OnUpdate, this, 1)); 


//Called by the world update start event 
public: void OnUpdate(const common::UpdateInfo& / * info* /) 
{ 
//Apply a small linear velocity to the model. 
this 一 > model - > SetLinearVel(math::Vector3(.03, 0, 0)); 
) 


//Pointer to the model 
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private: physics: :ModelPtr model; 


//Pointer to the update event connection 
private: event: :ConnectionPtrupdateConnection; 
}; 


//Register this plugin with the simulator 
GZ REGISTER MODEL PLUGIN(ModelPush) 
} 


2. 编译 插件 
假设 读者 已 经 读 过 Hello WorldPlugin 部 分 内 容 , 那 么 现在 所 需 做 的 是 添加 以 下 行 到 
一 /gazebo_plugin_tutorial/CMakeLists. txt 中 ,输入 以 下 命令 : 


add library(model push SHARED model push.cc) 
target link libraries(model push $ (GAZEBO LIBRARIES) $ (Boost LIBRARIES)) 


编译 此 代码 将 得 到 一 个 共享 库 , 一 /gazebo_plugin_tutorial/build/libmodel_push. so. tZ 
可 以 插入 到 Gazebo 仿真 中 ,输入 以 下 命令 : 


$ cd —/gazebo plugin tutorial/build 
$ cmake ../ 
$ make 


3. 运行 插件 
这 个 插件 用 在 场景 文件 examples/plugins/model_push/model_push. world 中 , 它 的 内 
容 为 : 


$ cd ~/gazebo plugin tutorial 
$ geditmodel push. world 

<?xml version = "1. 0"?> 

< sdf version = "1. 4"> 

< world name = "default"> 


<! — Ground Plane -一 > 

< include> 

< uri > model://ground_plane </uri> 
</include> 


< include> 
<uri>model://sun</uri> 


</include > 


< model name = "box"> 
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连接 插件 到 模型 的 钧 子 (hook) 由 下 列 语句 在 模型 元 素 块 的 结尾 指定 : 


将 用 户 的 库 路 径 添加 到 GAZEBO_PLUGIN_PATH ,输入 以 下 命令 : 


要 启动 仿真 ,输入 以 下 命令 : 


该 -u 选项 可 启动 处 于 暂停 状态 的 服务 器 。 
在 一 个 单独 的 终端 ,启动 gui, 输 入 以 下 命令 : 


单 击 gui 中 的 play 按钮 启动 仿真 ,用户 应 该 能 看 到 立方 体 移动 。 
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4.6.4 世界 插件 


1. 代码 
源 代码 为 : 


gazebo / examples / plugins / factory 


控制 运行 的 仿真 中 有 哪些 模型 以 及 何 时 插入 它们 可 能 是 有 用 的 。 本 部 分 内 容 演示 如 何 
将 预定 义 和 自 定义 模型 插入 到 Gazebo 中 。 
使 用 以 前 插件 部 分 内 容 中 的 gazebo_plugin_tutorial, 输 入 以 下 命令 : 


$ mkdir —/gazebo plugin tutorial 
$ cd —/gazebo plugin tutorial 


创建 新 的 源 文件 ,输入 以 下 命令 : 


$ gedit factory.cc 


将 以 下 代码 复制 到 factory. cc 文件 中 : 


# include < ignition/math/Pose3. hh> 
# include "gazebo/physics/physics. hh" 
include "gazebo/common/common. hh" 

# include "gazebo/gazebo. hh" 


namespace gazebo 
t 
class Factory : public WorldPlugin 
( 
public: void Load(physics::WorldPtr parent, sdf::ElementPtr / * _sdf * /) 
{ 
//Option 1: Insert model from file via function call. 
//The filename must be in the GAZEBO MODEL PATH environment variable. 
parent - > InsertModelFile("model://box"); 


//Option 2: Insert model from string via function call. 
//1nsert a sphere model from string 

Sdf::SDF sphereSDF; 

sphereSDF. SetFronString( 

"< sdf version = '1.4'>\ 

«model name = 'sphere'>\ 

«pose» 1000 0 0</pose>\ 

< link name = 'link'?V 
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<pose>00.5000</pose>\ 

<collision name = 'collision'?V 

< geometry >\ 

< sphere><radius> 0.5 </radius ></sphere >\ 

</geometry >\ 

</collision>\ 

< visual name = 'visual'>\ 

< geometry >\ 

< sphere><radius> 0.5 </radius ></sphere >\ 

</geometry >\ 

</visual >\ 

«/link»X 

«/model »X 

«/sd£ >"); 
//Demonstrate using a custom model name. 

sdf: :ElementPtr model = sphereSDF.Root() -> GetElenent( model"); 
model - > GetAttribute("name") —» SetFromString("unique sphere"); 
. parent - » InsertModelSDF( sphereSDF) ; 


//Option 3: Insert model from file via message passing. 
t 
//Create a new transport node 
transport: : NodePtr node(new transport: : Node( )) ; 
//1nitialize the node with the world name 
node -> Init( parent -> GetName( ) ) ; 


//Create a publisher on the 一 /factory topic 
transport::PublisherPtrfactoryPub = 
node - > Advertise < msgs: : Factory »(" —/factory"); 


//Create the nessage 
msgs::Factory msg; 


//Model file to load 
msg.set sdf filename("model://cylinder"); 


//Pose to initialize the model to 
msgs::Set(msg.mutable pose(), 
ignition: :math: :Pose3d( 
ignition::math::Vector3d(1, -2, 0), 
ignition: :math: :Quaterniond(0, 0, 0))); 


//Send the message 
factoryPub - > Publish(msg); 
} 
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E 


//Register this plugin with the simulator 
GZ REGISTER WORLD PLUGIN(Factory) 
) 


2. 代码 解释 
代码 的 第 一 部 分 创建 了 一 个 World 插件 。 


# include < ignition/math/Pose3. hh> 
# include "gazebo/physics/physics. hh" 
# include "gazebo/common/common. hh" 

# include "gazebo/gazebo. hh" 


namespace gazebo 
{ 
class Factory : public WorldPlugin 
t 
public: void Load(physics::WorldPtr parent, sdf::ElementPtr / * sdf » /) 


在 Load 函数 内 有 3 种 不 同 的 模型 插入 方法 。 
第 一 种 方法 使 用 World 方法 根据 GAZEBO MODEL PATH 环境 变量 定义 的 资源 路 
径 中 的 文件 加 载 模型 。 


//option 1: Insert model from file via function call. 
//The filename must be in the GAZEBO MODEL PATH environment variable. 
. parent - > InsertModelFile("model://box"); 


第 二 种 方法 使 用 World 方法 基于 字符 串 数据 加 载 模型 。 


//Option 2: Insert model from string via function call. 
//1nsert a sphere model from string 

sdf::SDF sphereSDF; 

SphereSDF. SetFromString( 

"< sdf version = '1.4'>\ 

«model nane = 'sphere'>\ 

<pose>1 0 0 0 0 0 </pose>\ 

< link name = 'link'>\ 

<pose>0 0 .5 0 0 0</pose>\ 

<collision name = 'collision'>\ 

< geometry >\ 

< sphere >< radius > 0.5 </radius ></ sphere >\ 

X/geonetry >\ 
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</collision>\ 

< visual name = 'visual'>\ 

< geometry >\ 

< sphere><radius> 0.5 </radius ></ sphere >\ 

</geometry >\ 

</visual >\ 

</link>\ 

</model »X 

</sdf >"); 
//Demonstrate using a custom model name. 

sdf: :ElementPtr model = sphereSDF.Root() -> GetElement( "model" ); 
model - > GetAttribute("name") - > SetFromString("unique sphere"); 
_parent - > InsertModelSDF(sphereSDF) ; 


第 三 种 方法 使 用 消息 传递 机 制 来 插入 模型 。 此 方法 对 通过 网 络 连接 与 Gazebo 通信 的 
独立 应 用 程序 最 有 用 。 


//Option 3: Insert model from file via message passing. 
t 

//Create a new transport node 

transport: :NodePtr node(new transport: : Node( ) ) ; 


//1Initialize the node with the world name 
node-» Init( parent 一 > GetName( )) ; 


//Create a publisher on the — /factory topic 
transport: :PublisherPtrfactoryPub = 
node - > Advertise < msgs: :Factory »(" —/factory"); 


//Create the message 
msgs: : Factory msg; 


//Model file to load 
msg.set sdf filename("model://cylinder"); 


//Pose to initialize the model to 
msgs::Set(msg.mutable pose(), 
ignition: :math: :Pose3d( 
ignition::math::Vector3d(1, -2, 0), 
ignition: :math: :Quaterniond(0, 0, 0))); 


//Send the message 
factoryPub - > Publish(nsg); 
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3. 编译 
假设 读者 已 经 读 过 插件 内 容 提要 部 分 内 容 , 那 么 只 需 保存 上 面 的 代码 为 : 


~/gazebo plugin tutorial/factory. cc 


并 添加 到 以 下 代码 中 : 


~/gazebo plugin tutorial/CMakeLists.txt 


输入 以 下 命令 : 


add library(factory SHARED factory.cc) 
target link libraries(factory 

$ (GAZEBO LIBRARIES) 
) 


编译 此 代码 将 得 到 一 个 共享 库 一 /gazebo_plugin_tutorial/build/libfactory. so, 它 可 以 
插入 到 Gazebo 仿真 中 ,输入 以 下 命令 : 


mkdir ~/gazebo plugin tutorial/build 
cd ~/gazebo plugin tutorial/build 
cmake ../ 

make 


4 Xp Xo 4 


4. 例子 : 几何 体 的 制作 
编译 包含 立方 体 和 圆柱 体 的 模型 目录 .输入 以 下 命令 : 


$ mkdir —/gazebo plugin tutorial/models 
$ cd —/gazebo plugin tutorial/models 
$ mkdir box cylinder 


创建 立方 体 模型 ,输入 以 下 命令 : 


$ cd box 
$ geditmodel. sdf 


将 以 下 内 容 复 制 并 粘贴 到 box/model. sdf 中 : 


<?xml version = '1.0'?> 

< sdf version = '1.6'> 

< model name = 'box'> 

<pose>1 2 0 0 0 0 </pose> 

< link name = 'link'> 

<pose>0 0.5 0 0 0</pose> 
<collision name = 'collision'> 
< geometry > 
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创建 model. config 文件 ,输入 以 下 命令 : 


将 以 下 内 容 复制 到 model. config: 


导航 到 圆柱 体 目 录 , 并 创建 一 个 新 model. sdf 文件 ,输入 以 下 命令 : 


将 以 下 内 容 复制 到 model. sdf: 


466 4| 机 器 人 仿真 与 编程 技术 


创建 一 个 model. config 文件 ,输入 以 下 命令 : 


将 以 下 内 容 复制 到 model. config 中 : 


运行 代码 ,确保 用 户 的 $GAZEBO_MODEL_PATH 指 的 是 用 户 的 新 模型 目录 ,输入 以 


将 用 户 的 库 路 径 添加 到 GAZEBO_PLUGIN_PATH, 输 入 以 下 命令 : 
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$ export GAZEBO PLUGIN PATH= $ HOME/gazebo plugin tutorial/build: $ GAZEBO PLUGIN PATH 


创建 一 个 场景 SDF 文件 ,命名 为 一 /gazebo_plugin_tutorial/factory. world, 输 入 以 下 


$ cd —/gazebo plugin tutorial 
$ geditfactory. world 


将 以 下 内 容 复制 到 场景 中 : 

<?xml version = "1.0"?> 

< sdf version = "1.4"> 

< world name = "default" 

< include> 

<uri> model://ground plane</uri> 
</include> 

< include> 
<uri>model://sun</uri> 
</include> 

< plugin name = "factory" filename = "libfactory. so"/> 


</world> 
</sdf > 


运行 Gazebo ,输入 以 下 命令 : 
$ gazebo —/gazebo plugin tutorial/factory.world 


Gazebo 窗口 应 显示 这 样 一 个 场景 : 一 个 球体 .立方体 和 一 个 圆柱 体 排列 为 一 行 。 这 个 
插件 示例 以 编程 方式 修改 重力 。 


4.6.5 程序 化 场景 控制 


1. 安装 
使 用 以 前 的 插件 部 分 内 容 中 的 gazebo_plugin_tutorial .输入 以 下 命令 : 


$ mkdir —/gazebo plugin tutorial; cd ~ /gazebo plugin tutorial 
j£ —4- 44 3 — /gazebo plugin tutorial/world edit. world 的 文件 ,输入 以 下 命令 : 


$ geditworld edit. world 
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向 其 中 添加 以 下 内 容 : 


2. 代码 
创建 一 个 名 为 一 /gazebo_plugin_tutorial/world_edit. cc 的 文件 ,输入 以 下 命令 : 


向 其 中 添加 以 下 内 容 : 
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代码 解释 如 下 : 


我 们 创建 一 个 新 的 节点 指针 ,并 将 其 初始 化 为 使 用 场景 名 称 。 场 景 名 称 允 许 节点 与 一 
个 特定 场景 通信 。 


创建 以 "一 / physics” 主 题 发 送 物理 消息 的 发 布 服务 器 (publisher) 。 
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physicsMsg.set max step size(0.01); 
//Change gravity 


msgs::Set(physicsMsg.mutable gravity(), ignition: :math: :Vector3d(0. 01, 0, 0.1)); 
physicsPub - > Publish(physicsMsg); 


创建 物理 消息 ,并 改变 步 长 时 间 和 重力 。 然 后 将 此 消息 发 布 到 "一 / physics" 3:88. 
3. 构建 
假设 读者 已 经 读 过 插件 内 容 提要 部 分 内 容 ,那么 只 需 保 存 上 面 的 代码 为 ， 


—/gazebo plugin tutorial/world edit.cc 
并 添加 以 下 行 到 一 /gazebo_plugin_tutorial/CMakeLists. txt 中 ,输入 以 下 命令 : 


add library(world edit SHARED world edit.cc) 
target link libraries(world edit $ (GAZEBO LIBRARIES)) 


编译 此 代码 将 得 到 一 个 共享 库 , — /gazebo, plugin. tutorial/build/libworld. edit. so. È 
可 以 插入 到 Gazebo 仿真 中 ,输入 以 下 命令 : 

$ mkdir ~/gazebo plugin tutorial/build 

$ cd ~/gazebo plugin tutorial/build 


$ cmake ../ 
$ make 


4. 运行 部 分 内 容 
首先 需要 将 文件 夹 添加 到 GAZEBO PLUGIN PATH 环境 变量 中 ,输入 以 下 命令 : 


export GAZEBO PLUGIN PATH - $ (GAZEBO PLUGIN_PATH} :~/gazebo plugin tutorial/build/ 


然后 在 终端 上 输入 下 列 命令 ,输入 以 下 命令 : 


$ cd —/gazebo plugin tutorial 
$ gazebo world edit.world 


用 户 应 该 看 到 一 个 空 场景 。 
现在 使 用 位 于 泻 染 窗口 上 方 的 Box 图 标 向 场景 中 添加 一 个 立方 体 。 立 方 体 应 该 会 向 
上 浮动 ,并 远离 相机 。 


4.6.6 系统 插件 
本 部 分 内 容 将 创建 一 个 源 文件 , 它 是 gzclient 的 系统 插件 ,用 于 将 图 像 保 存 到 目录 
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/tmp/gazebo_frames 中 。 


我 们 将 从 源 文件 开始 。 使 用 以 下 内 容 创建 一 个 名 为 system. gui. cc 的 文件 ,输入 以 下 
命令 : 


将 以 下 内 容 复 制 到 system. gui. cc: 
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private: void Update() 


t 
if (!this -> userCam) 
t 
//Get a pointer to the active user camera 
this 一 > userCam = gui::get active camera(); 


//Enable saving frames 

this 一 > userCam - > EnableSaveFrame(true); 

//Specify the path to save frames into 

this -> userCam 一 > SetSaveFramePathname(" /tmp/gazebo frames"); 


} 


//Get scene pointer 
rendering::ScenePtr scene = rendering::get scene(); 


//Wait until the scene is initialized. 
if (!scene || ! scene - » Initialized()) 
return; 


//Look for a specific visual by name. 
if (scene -> GetVisual("ground plane")) 
std: :cout ««"Has ground plane visualWn"; 
) 


// [Pointer the user camera. 
private: rendering: :UserCameraPtruserCam; 


// /M1 the event connections. 
private: std: : vector < event: :ConnectionPtr? connections; 


E 
//Register this plugin with the simulator 


GZ REGISTER SYSTEM PLUGIN(SystenGUI) 
) 


Load 和 Init 函数 一 定 不 能 被 限制 。Load 和 Init 函数 在 启动 时 、Gazebo 加 载 之 前 被 调 
用 。 第 一 次 Update 时 ,我 们 得 到 一 个 指向 用 户 相机 (图 形 界面 中 使 用 的 相机 ) 的 指针 ,并 局 
用 帧 保存 。 

COD 获取 用 户 相 机 : 


this 一 >userCam = gui::get active camera(); 
(2) 启用 保存 帧 : 


this 一 > userCam 一 > EnableSaveFranme(true); 
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G) 设置 保存 帧 的 位 置 : 


this 一 > userCam - > SetSaveFramePathname("/tmp/gazebo frames"); 


1) 编译 相机 插件 
假设 读者 已 经 读 过 Hello WorldPlugin 部 分 内 容 , 那 么 只 需 添 加 以 下 行 到 一 /gazebo_ 
plugin tutorial/CMakeLists. txt 中 。 输 入 以 下 命令 : 


add library(system gui SHARED system gui.cc) 
target link libraries(system gui $ {GAZEBO LIBRARIES}) 


重建 ,用 户 最 终 应 该 得 到 一 个 libsystem_gui. so FE ,输入 以 下 命令 : 
$ cd ~/gazebo plugin tutorial/build 


$ cmake ../ 
$ make 


2) 运行 插件 

首先 ,在 后 台 启 动 gzserver, 输 入 以 下 命令 : 

$ gzserver& 

使 用 插件 运行 客户 端 ,输入 以 下 命令 : 

$ gzclient -g libsystem gui.so 

fE/tmp/gazebo frames 里 面 ,用 户 应 该 看 到 许多 从 当前 插件 保存 的 图 像 。 

注意 : 记 住 还 要 在 退出 客户 端 后 终止 后 台 服务 器 进程 。 在 同一 终端 ,将 进程 带 到 前 台 ， 
输入 以 下 命令 : 

$ fg 

并 按 Ctrl 十 C 组 合 键 中止 该 过 程 。 或 者 ,只 需 结 束 gzserver 进程 ,输入 以 下 命令 : 


$ killallgzserver 


4.7 传感器 


4.7.1 传感器 噪声 模型 
Gazebo 提供 许多 常见 传感器 的 型 号 。 在 现实 世界 中 ,传感器 存在 噪声 ,是 因为 它们 能 
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完全 反映 实际 。 理 想 状 态 下 ,默认 Gazebo 的 传感器 能 够 完全 反映 实际 (虽然 不 是 IMU)。 

为 了 呈现 一 个 更 现实 的 环境 ,尝试 在 其 中 加 入 感知 代码 ,我 们 需要 添加 噪声 到 Gazebo 
的 传感器 生成 的 数据 中 。 

Gazebo 可 以 给 以 下 类 型 的 传感器 添加 噪声 : 

。 射线 (例如 ,激光 ) 

。 摄像头 

* IMU 

1. 镭射 (激光 ) 了 噪声 

对 于 射线 传感器 ,向 每 个 射 束 的 范围 添加 高 斯 噪声 。 用 户 可 以 设置 采样 噪声 值 的 高 斯 
分 布 的 均值 和 标准 偏差 。 每 个 波束 独立 地 对 噪声 值 进行 采样 。 添 加 噪声 后 ,结果 会 在 传 感 
器 的 最 小 和 最 大 范围 (包括 ) 之 间 。 

测试 射线 噪声 模型 

(1) 创建 模型 目录 ,输入 以 下 命令 : 


mkdir -p ~/.gazebo/models/noisy laser 


(2) 创建 模型 配置 文件 ,输入 以 下 命令 : 


gedit —/.gazebo/models/noisy laser/model.config 


(3) 添加 以 下 内 容 : 


<?xml version = "1.0"?> 

< model > 

< name > Noisy laser </name > 

« version» 1.0 </version> 

< sdf version = '1.6'» model. sdf </sdf > 


< author > 

< name > My Name </name > 

< email > me@my. email </email > 
</author > 


<description> 
My noisy laser. 


</description> 
</model > 


(4) 创建 一 /. gazebo/models/noisy_laser/model. sdf 文件 ,输入 以 下 命令 : 


gedit 一 /. gazebo/models/noisy laser/model. sdf 
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C5) 添加 如 下 程序 ,这 是 一 个 加 上 噪声 的 标准 Hokuyo 模型 的 副本 : 
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(6) 启动 Gazebo, 插 入 带 噪声 的 激光 器 ,如 图 4-51 所 示 , 在 左 窗 格 中 


IX JP 


“噪声 激光 ”按钮 。 将 激光 器 放 在 场景 中 某 个 位 置 。 


Ph ,选择 * 捅 入 "选项 


图 4-51 激光 器 的 插入 


(7) 可 视 化 带 噪声 的 激光 器 : 如 图 4-52 所 示 , 依 次 单 击 窗口 .主题 可 视 化 按钮 (或 按 下 
Ctrl 十 工 组 合 键 ) 以 显示 主题 选择 器 。 


O Gazebo: Topic Selector 


图 4-52 主题 选择 器 对 话 框 
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(8) 找到 名 称 为 /gazebo/default/hokuyo/link/laser/scan 的 选项 , 单 击 它 , 然 后 单 击 “ 确 
定 ” 按 钮 。 如 图 4-53 所 示 ,用 户 会 得 到 一 个 激光 视图 窗口 ,显示 激光 数据 。 


图 4-53 显示 激光 数据 的 窗口 


用 户 可 以 看 到 ,显示 是 带 噪声 的 。 为 了 调整 噪声 ,只 需 使 用 model. sdf 中 的 平均 值 和 标 
准 偏差 值 ,其 中 单位 为 米 。 设 置 以 下 内 容 : 


<noise> 

<type> gaussian </type> 
<mean>0.0</mean> 

< stddev> 0. 01 </stddev > 
</noise> 


这 些 是 Hokuyo 激光 器 的 合理 值 。 

2. 摄像 头 噪音 

对 于 摄像 头 传感器 ,我 们 对 输出 放大 器 噪声 进行 建 模 ,其 对 每 个 像素 独立 地 添加 高 斯 采 
样 干扰 。 用 户 可 以 设置 将 被 采样 的 噪声 值 的 高 斯 分 布 的 均值 和 标准 偏差 。 针 对 每 个 像素 独 
立地 对 噪声 值 进 行 采 样 ,然后 将 该 噪声 值 独立 地 添加 到 该 像素 的 每 个 颜色 通道 。 在 添加 噪 
声 之 后 ,所 得 到 的 颜色 通道 值 被 错位 在 0.0 和 1.0 之 间 ; 此 浮 点 颜色 值 将 在 图 像 中 作为 无 符 
号 整数 ,通常 在 0 和 255 之 间 ( 每 个 通道 使 用 8 位 )。 
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这 个 噪声 模型 在 GLSL 着 色 器 中 实现 ,需要 GPU 运行 。 
测试 摄像 头 噪声 模型 时 : 
CD 创建 模型 目录 ,输入 以 下 命令 : 


(2) 创建 模型 配置 文件 ,输入 以 下 命令 : 


(3) 复制 粘贴 以 下 内 容 : 


(4) 创建 一 /. gazebo/models/noisy. camera/model. sdf 文件 ,输入 以 下 命令 : 


C5) 添加 以 下 内 容 , 这 是 添加 了 噪声 的 标准 摄像 头 型 号 的 副本 : 
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< visual name = "visual"> 
<geometry> 

<box> 
<size>0.10.10.1</size> 
</box> 

</geometry> 

</visual > 

< sensor name = "camera" type = "camera"> 
<camera> 

< horizontal_fov> 1. 047 </horizontal_fov > 
< image > 

< width» 1024 </width> 

< height > 1024 </height > 
</image> 

<clip> 

< near > 0. 1 </near > 

< far > 100 </far > 

</clip> 

<noise> 

< type > gaussian </type > 

< mean > 0. 0 </mean > 

< stddev > 0. 07 </stddev > 
</noise> 

</camera > 

<always_on> 1 </always_on > 

< update rate» 30 </update_rate > 
< visualize» true </visualize> 
</sensor > 

</link> 

</model> 

</sdf > 


(6) 启动 Gazebo, 然 后 插入 带 噪声 的 摄像 头 : 在 左 窗 格 中 ,选择 “插入 ”选项 卡 , 然 后 单 
击 “ 品 声 摄像 头 ” 按 钮 。 把 用 户 的 摄像 头 放 在 场景 的 某 个 地 方 。 

(7) 可 视 化 带 噪声 的 摄像 头 : 依次 单 击 窗口 .主题 可 视 化 按钮 (或 按 Ctrl 十 工 组 合 键 ) 以 
显示 主题 选择 器 。 

(8) 找到 名 称 为 /gazebo/default/camera/link/camera/image” 的 选项 , 单 击 它 , 然 后 单 
击 “ 确 定 ” 按 钮 。 如 图 4-54 所 示 ,用 户 会 得 到 一 个 图 像 视图 窗口 ,显示 图 像 数 据 。 

不 难 发 现 , 图 像 是 有 噪声 的 。 要 调整 噪声 ,只 需 调 整 在 model. sdf 中 的 平均 值 和 标准 偏 
差 值 。 这 些 是 无 单位 的 值 ; 噪声 将 被 添加 到 范围 [0. 0,1.0] 内 的 每 个 颜色 通道 。 

上 面 的 例子 有 很 高 的 < stddev>。 尝 试 减 小 此 值 , 以 下 的 设置 参数 为 数字 摄像 头 的 合理 值 : 
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Gazebo: Image View 


图 4-54 显示 图 像 数 据 的 窗口 


<noise> 

< type > gaussian </type > 
<mean>0.0</mean> 

< stddev > 0. 007 </stddev > 
</noise> 


这 些 是 数字 摄像 头 的 合理 值 。 

3. IMU 噪声 

对 于 IMU 传感器 ,我 们 对 两 种 对 角速度 和 线性 加 速度 的 干扰 进行 仿真 : 噪声 和 偏 置 。 
角 速 率 和 线性 加 速度 单独 考虑 ,引出 该 模型 的 4 组 参数 : 速率 噪声 ,速率 偏差 ,加 速度 噪声 
和 加 速度 偏差 。 没 有 噪声 施加 到 IMU 的 方向 数据 ,将 其 作为 在 坐标 中 的 理想 值 来 处 理 。 

噪声 是 可 加 的 ,从 高 斯 分 布 采 样 。 用 户 可 以 设置 高 斯 分 布 的 平均 值 和 标准 偏差 (一 个 用 
于 速率 ,一 个 用 于 加 速度 ) ,从 中 可 以 对 噪声 值 进行 采样 。 针 对 每 个 样本 的 每 个 分 量 (X,Y， 
Z) 独 立地 对 噪声 值 进行 采样 ,并 将 其 添加 到 该 分 量 。 

偏差 也 是 可 加 的 ,但 在 仿真 开始 时 , 它 被 采样 一 次 。 用 户 可 以 设置 高 斯 分 布 的 平均 值 和 
标准 偏差 (一 个 用 于 速率 , 另 一 个 用 于 加 速度 ) .从 中 可 以 对 偏差 值 进 行 采样 。 偏 置 将 根据 提 
供 的 参数 进行 采样 ,然后 以 等 概率 进行 否定 :假设 所 提供 的 平均 值 指示 偏差 的 幅度 ,并 且 它 
可 能 偏向 任 一 方向 。 此 后 ,偏差 是 固定 值 ,加 到 每 个 样品 的 每 个 分 量 (X,Y,Z) 。 

注意 : 根据 被 仿真 的 系统 和 物理 引擎 的 配置 ,可 能 发 生 仿真 的 IMU 数据 已 经 包含 相当 
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的 骂 声 ,因为 系统 不 是 完全 收 化 。 因 此 ,根据 用 户 的 实际 应 用 情况 ,可 能 不 需要 添加 噪声 。 
接 下 来 对 IMU 噪声 模型 进行 测试 。 
CD 创建 模型 目录 ,输入 以 下 命令 : 


(2) 创建 模型 配置 文件 ,输入 以 下 命令 : 


(3) 在 文件 中 加 入 以 下 内 容 : 


(4) 创建 一 /. gazebo/models/noisy_imu/model. sdf 文件 ,输入 以 下 命令 : 


(5) 添加 以 下 内 容 : 
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“噪声 IMU” 按 钮 。 用 户 将 IMU 放 在 场景 中 的 某 个 地 方 。 


H 


的 单位 为 m/s“ 2。 
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<bias stddev> 0.001 </bias stddev> 
</noise> 

</x> 

<y> 

<noise type = "gaussian"> 

< nean > 0. 0 «/nean 

< stddev > 1. 7e - 2 </stddev > 

Xbias mean» 0.1«/bias mean 

Xbias stddev» 0.001 </bias_stddev > 
</noise> 

</y> 

<z> 

< noise type = "gaussian"> 

< mean > 0. 0 </mean > 

< stddev > 1. 7e - 2 </stddev > 

<bias mean> 0. 1 </bias_mean> 
<bias_stddev> 0.001 </bias_stddev > 
</noise> 

</z> 

</linear_acceleration> 

</imu> 

<always_on> 1 </always_on > 

«update rate? 1000 </update rate> 
</ sensor > 

</link> 

</model > 

</sdf > 
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(6) 启动 Gazebo, 然 后 插入 带 噪 声 的 IMU, 在 左 窗 格 中 ,选择 “插入 ”选项 卡 ,然后 单 击 


CD 可 视 化 带 噪声 的 IMU: 依次 单 击 窗口 .主题 可 视 化 按钮 (或 按 Ctrl 十 T 组 合 键 ) 以 
显示 主题 选择 器 。 
(8) 找到 名 称 为 /gazebo/default/imu/link/imu/imu” 的 选项 , 单 击 它 ,然后 单 击 “ 确 
定 ” 按 钮 。 用 户 会 得 到 一 个 文本 视图 窗口 ,显示 用 户 的 IMU 数据 。 

在 高 速率 的 传感器 上 加 入 噪声 是 很 难 让 人 接受 的 ,特别 是 在 复杂 系统 中 。 用 户 应 该 能 
够 在 噪声 和 /或 偏 置 参数 中 看 到 大 的 非 零 均 值 的 影响 。 要 调整 噪声 ,只 需 调整 在 model. sdf 


<angular velocity> 
<x> 
< noise type = "gaussian"> 


bh 的 平均 值 和 标准 偏差 值 。 速 率 噪声 和 速率 偏差 的 单位 为 rad/s, 加 速度 噪声 和 加 速度 偏 移 
以 下 的 参数 设置 为 高 质量 的 IMU 的 合理 值 : 
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4.7.2. 接触 式 传感器 


本 部 分 内 容 演 示 了 创建 传感器 并 通过 插件 或 消息 获取 数据 的 过 程 。 接 触 式 传感器 检测 
两 个 物体 之 间 的 碰撞 并 报告 接触 相关 力 的 位 置 。 

1. 安装 部 分 内 容 

创建 工作 目录 开始 ,输入 以 下 命令 : 


接 下 来 ,创建 包含 接触 式 传感器 的 盒子 的 SDF 世界 文件 ,输入 以 下 命令 : 


将 以 下 代码 复制 到 名 为 contact. world 的 文件 中 : 
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接触 式 传感器 将 装 到 到 箱 模 型 内 的 连 杆 上 。 它 将 检测 到 box_collision 对 象 与 场景 中 
任何 其 他 对 象 之 间 的 冲突 。 

2. 打印 接触 值 

使 用 Gazebo 运行 contact. world, 输 入 以 下 命令 ， 


在 一 个 单独 的 终端 列表 中 ,由 Gazebo 产生 的 标题 为 : 


输出 应 如 下 所 示 : 
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/gazebo/default/box/link/my_contact 
/gazebo/default/box/link/my contact/contacts 
/gazebo/world/modify 
/gazebo/default/diagnostics 
/gazebo/default/factory 
/gazebo/default/model/nmodify 
/gazebo/default/scene 
/gazebo/default/physics 
/gazebo/default/world control 
/gazebo/server/control 


我 们 关注 的 标题 是 /gazebo/default/box/link/my_contact。 在 这 里 , my_contact 接触 
式 传感器 根据 这 个 标题 发 布 。 
将 接触 式 传感器 的 值 打印 到 屏幕 上 ,输入 以 下 命令 : 


gz topic — e /gazebo/default/box/link/my contact 
上 面 的 命令 将 所 有 的 接触 转 储 到 终端 。 用 户 可 以 使 用 Ctrl 十 C 组 合 键 随 时 停止 。 
注意 ; 如 果 该 命令 无 效 , 用 户 可 能 需要 在 </contact > 和 </sensor > 标签 之 间 添 加 


<update_rate> 5 </update_rate> 


以 获得 在 终端 上 的 输出 。 根 据 不 同 的 场合 ,速率 “5” 可 以 改变 为 不 同 的 频率 。 

3. 获取 传感器 数据 插件 

用 户 也 可 以 为 接触 式 传感器 创建 插件 。 这 个 插件 可 以 获得 操作 碰撞 数据 ,并 将 其 输出 
接 到 任意 目标 (例如 ROS 主题 ) 。 

首先 修改 contact. worldSDF 文件 。 在 下 面 添加 以 下 行 : 

< sensor name = 'my contact' type = 'contact'>: 


geditcontact. world 
< plugin name = "ny plugin" filename = "libcontact. so"/> 


这 一 行 告诉 Gazebo 加 载 我 们 现在 将 定义 的 libeontact. so 传感器 插件 。 
为 插件 创建 一 个 头 文件 ,命名 为 ContactPlugin. hh: 


geditContactPlugin. hh 
并 粘贴 以 下 内 容 : 


# ifndef GAZEBO CONTACT PLUGIN HH 
井 define GAZEBO CONTACT PLUGIN HH 
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创建 一 个 名 为 ContactPlugin. cc 的 源 文 件 : 


并 粘贴 以 下 内 容 : 
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TUB MH M M HP LM PH P P M M M P 
ContactPlugin::ContactPlugin() : SensorPlugin() 

t 

) 


HH HM HM HL P M HL LL I M CM 
ContactPlugin::-—ContactPlugin() 

t 

} 


HH HM HM M M HH M I Gg 
voidContactPlugin::Load(sensors::SensorPtr sensor, sdf::ElementPtr / * _sdf /) 
í 

//Get the parent sensor. 
this 一 > parentSensor = 
std: :dynamic pointer cast sensors::ContactSensor?( sensor); 


//Make sure the parent sensor is valid. 
if (! this -> parentSensor) 

{ 
gzerr <<"ContactPlugin requires a ContactSensor. Wn" ; 
return; 


) 


//Connect to the sensor update event. 
this - > updateConnection = this -  parentSensor - > ConnectUpdated( 
std: :bind(&ContactPlugin::OnUpdate, this)); 


//Make sure the parent sensor is active. 
this -  parentSensor - > SetActive(true); 
) 


HH PL I PH PM M HH M P M P gg 
voidContactPlugin: :OnUpdate( ) 
( 
//Get all the contacts. 
msgs: : Contacts contacts; 
contacts = this -> parentSensor - > Contacts() ; 
for (unsigned int i = 0; i «contacts.contact size(); ++i) 
t 
std: :cout ««"Collision between["«« contacts. contact(i).collisionl() 
««" ] and [ "<< contacts. contact(i).collision2() ««"]Wn"; 


for (unsigned int j = 0; j «contacts.contact(i).position size(); **j) 
i 
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std: :cout << j <<" Position:" 
«X contacts.contact(i).position(j).x() <<"" 
<< contacts.contact(i).position(j).y() ««"" 
«€ contacts. contact(i).position(j).z() <<"\n"; 
std: :cout <<"  Normal:" 
<< contacts. contact(i).normal(j).x() <<"" 
<< contacts. contact(i).normal(j).y() <<"" 
<< contacts. contact(i).normal(j).z() <<"\n"; 
std: :cout <<" Depth: "<< contacts. contact (i).depth(j) <<"\n"; 
} 
) 
} 


4. 编译 代码 
(D 创建 CMakeLists. txt 文件 ,输入 以 下 命令 : 


cd —/gazebo contact tutorial; gedit CMakeLists.txt 


(2) 复制 以 下 代码 并 保存 文件 : 

cmake minimum required(VERSION 2.8 FATAL ERROR) 
find package(gazebo REQUIRED) 

include directories( $ (GAZEBO INCLUDE DIRS]) 
link directories( $ (GAZEBO LIBRARY DIRS}) 


list(APPEND CMAKE CXX FLAGS " $ (GAZEBO CXX FLAGS]") 


add library(contact SHARED ContactPlugin.cc) 
target link libraries(contact $ (GAZEBO libraries]) 


(3) 接 下 来 ,创建 一 个 构建 目录 并 创建 插件 .输入 以 下 命令 : 


mkdir build; cd build; cmake ../; make 


5. 运行 代码 
CO 输入 构建 目录 ,输入 以 下 命令 : 


cd ~/gazebo_contact tutorial/build 


(2) 运行 gzserver, 首 先 修改 用 户 LD LIBRARY PATH 的 库 , 以 便 库 加 载 器 可 以 找到 
户 的 库 ( 默 认 情况 下 它 只 会 查看 某 些 系统 位 置 ) .输入 以 下 命令 : 
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export LD LIBRARY PATH= ~/gazebo contact tutorial/build: $ LD LIBRARY PATH 
gzserver .. /contact. world 


4.7.3. 摄像 头 失 真 


摄像 机 镜头 通常 表现 出 一 定 程 度 的 光学 失真 ,这 导致 图 像 的 竹 曲 。 本 部 分 内 容 示 例 是 
鱼 眼 摄像 头 , 其 通常 用 于 机 器 人 应 用 中 ,以 获得 用 于 物体 识别 或 导航 任务 的 环境 的 更 宽 视 
场 。 使 用 诸如 MATLAB sk OpenCV 的 摄像 头 校准 工具 ,可 以 提取 失真 系数 以 及 其 他 摄像 
头 内 在 参数 。 使 用 这 些 失 真 系数 ,用 户 现在 可 以 在 Gazebo 中 创建 失真 的 摄像 头 传感器 。 

1. 当前 实施 方案 

Gazebo 目前 支持 基于 布朗 失真 模型 的 摄像 头 仿真 。 该 公司 要 求 的 5 个 畸变 系数 (kl、 
k2, k3, pl,p2) ,用户 可 以 从 摄像 头 校准 工具 获得 。k 系数 是 畸变 模型 的 径 向 分 量 , 而 p 系数 
是 切 向 分 量 。 

需要 考虑 的 当前 实现 有 一 些 限制 : 

(1) 目前 仅 支 持 桶 形 畸 变 , 这 通常 具有 负 值 k1。 

(2) 失真 应 用 于 摄像 头 图 像 纹 理 。 这 意味 着 获取 生成 的 图 像 数 据 并 且 只 是 使 它 变形 。 
这 需要 注意 的 是 ,最 终 的 图 像 (特别 是 在 角落 ) 比 具有 桶 形 畸 变 的 真实 摄像 头 镜头 的 视野 更 
窗 。 补 偿 此 效果 的 一 种 解决 方法 是 增加 Gazebo 中 摄像 头 传感器 的 视野 。 

2. 创建 带 有 畸变 的 摄像 头 

要 添加 带 畸 变 的 摄像 头 型 号 ; 

(1) 创建 模型 目录 ,输入 以 下 命令 : 


mkdir -p —/.gazebo/models/distorted camera 


(2) 创建 模型 配置 文件 ,输入 以 下 命令 : 


gedit —/.gazebo/models/distorted camera/model.config 


(3) 添加 以 下 内 容 : 


<?xml version = "1.0"?> 

< model > 

< name > Distorted Camera </name > 

X version» 1.0 </version> 

< sdf version = '1.5'» model. sdf </sdf > 


< author > 
< name > My Name </name > 
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(4) 创建 一 /. gazebo/models/distorted camera/model. sdf 文件 。 


(5) 添加 以 下 内 容 , 这 是 一 个 附加 的 失真 的 标准 摄像 头 模型 的 副本 : 
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<width> 320 </width> 

< height > 240 </height > 
</image> 

<clip> 

<near>0.1</near> 
<far>100 </far> 

</clip> 

<distortion> 
<k1>-0.25</k1> 
<k2>0.12</k2> 
<k3>0.0</k3> 

< pl >- 0. 00028 «/p1 > 
<p2>- 0.00005 </p2> 

X center» 0.5 0.5 </center> 
«/distortion» 

X/camera > 

«always on»1«/always on» 
< update rate» 30 «/update rate? 
< visualize» true </visualize > 
</sensor > 

</link> 

</model > 

</sdf > 


(6) 启动 Gazebo, 然 后 插入 畸变 的 摄像 头 型 号 : 在 左 窗 格 中 ,选择 Insert 选项 卡 ,然后 
单 击 Distorted Camera 按钮 。 把 用 户 的 摄像 头 放 在 场景 的 某 个 地 方 ,并 在 它 前 面 放 一 个 盒子 。 

CD 观看 摄像 头 图 像 : 依次 单 击 窗口 .主题 可 视 化 按钮 (或 按 Ctrl 十 工 键 ) 以 显示 主题 选 
择 器 。 

(8) 找到 标题 为 /gazebo/default/distorted_camera/link/camera/image, 单 击 它 ,然后 单 
i OK 按钮 。 用 户 会 得 到 一 个 摄像 机 视图 窗口 .显示 摄像 机 图 像 数 据 。 
正如 用 户 所 看 到 的 ,从 盒子 弯曲 的 边缘 可 见 ,摄像 机 的 图 像 是 畸变 的 。 要 调整 失真 ,只 
需 调整 model. sdf 中 kl1、k2、k3、pl、pl 失真 系数 。 


4.8 Gazebo 的 其 他 功能 


4.8.1 数学 库 的 使 用 


Gazebo 有 一 个 自 定义 的 数学 库 。 这 部 分 内 容 描 述 如 何 使 用 这 些 数学 函数 。 

Gazebo 使 用 随机 数 发 生 器 。 默 认 情 况 下 ,种 子 设置 为 运行 Gazebo 的 进程 的 PID。 也 
可 以 手动 设置 随机 数 种 子 。 该 特征 的 优点 是 获得 随机 数 的 确定 性 序列 ,这 对 于 测试 可 重复 
性 是 有 益 的 。 
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1. 命令 行 


Gazebo 可 以 使 用 命令 行 上 的 随机 数 种 子 来 初始 化 ,使 用 -seed 参数 : 


gazebo -- seed < integer > 


2. 信息 
要 使 用 Gazebo 监听 一 /world_control 主题 , 则 需要 类 型 为 msgs :: WorldControl 的 消 
息 。world 控制 消息 可 能 包含 随机 数 种 子 。 


4.8.2 用 户 输入 


用 户 输入 可 以 采取 多 种 形式 ,包括 来 自 图 形 界面 和 诸如 操纵 杆 的 硬件 设备 。 

1. Razer Hydra 

(1) Gazebo 支持 Razer Hydra 控制 器 。 用 户 将 能 够 使 用 此 运动 和 方向 检测 控制 器 与 
Gazebo 中 的 模型 进行 交互 。 

(2) Razer Hydra 配置 。 

打开 终端 (CTRL-ALT-T) ,然后 输入 以 下 命令 : 

echo — e "ATTRS( idProduct) == \"0300\", ATTRS{ idVendor) == \"1532\", ATTR( bInterfaceNumber] 


== \"00\", TAG = V" hydra - trackerV " V nSUBSYSTEM == V" hidrawV", TAGS == V hydra - trackerV", 
MODE = \ "0666\", SYMLINK += \"hydra\"" > 90 - hydra. rules 


这 将 创建 一 个 名 为 90-hydra. rules 的 文件 。 
我 们 能 够 访问 控制 器 ,无须 root 访问 权限 ,需要 访问 控制 器 时 输入 以 下 命令 : 


sudocp 90 - hydra. rules /etc/udev/rules.d/ 
sudoudevadm control —- reload - rules 


(3) Razer Hydra 下 编辑 Gazebo。 
我 们 需要 安装 libusb 的 从 属 项 ,输入 以 下 命令 : 


sudo apt - get install libusb- 1.0 - 0 - dev 


—H Hydra 配置 成 功 并 且 满足 附加 从 属 关系 ,用 户 则 能 够 在 Hydra 下 编译 Gazebo。 
按照 此 说 明 编 译 Gazebo。 在 执行 cmake 命令 期 间 , 用 户 应 该 看 到 此 确认 找到 SDK 
消息 : 


—— Looking for libusb- 1.0 - found. Razer Hydra support enabled. 


(4) 在 Gazebo 内 使 用 Hydra, YE Gazebo 中 使 用 Hydra 需要 两 个 步骤 。 第 一 步 是 将 
Hydra 插件 加 载 到 用 户 的 world 文件 中 ,输入 以 下 内 容 : 
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<! —— Load the plugin for Razer Hydra -一 > 
< plugin name = "hydra" filename = "libHydraPlugin. so"></plugin > 


此 插件 将 自动 发 布 有 关 一 /hydra 的 消息 。 

第 二 步 将 使 用 Hydra 的 右 操纵 杆 移 动 球体 。 可 用 plugins 目录 下 的 一 个 名 为 
HydraDemoPlugin 的 插件 上 . 

插件 代码 为 : 


# include <boost/bind.hpp> 

# include < gazebo/gazebo. hh> 

# include < gazebo/physics/physics. hh> 

# include "gazebo/transport/transport. hh" 
# include "HydraDemoPlugin. hh" 


using namespace gazebo; 
GZ REGISTER MODEL PLUGIN(HydraDemoPlugin) 


A 
HydraDemoPlugin: :HydraDemoPlugin() 
{ 


HMM HL LH VG G 
HydraDemoPlugin: :~ HydraDemoPlugin() 

{ 

) 


HIM M P M 
voidHydraDemoPlugin: :OnHydra(ConstHydraPtr& msg) 

{ 

boost::mutex::scoped lock lock( this - > msgMutex); 
this 一 > hydraMsgPtr = _msg; 

) 


HU P M P à n n:14 
voidHydraDemoPlugin::Load(physics::ModelPtr parent, sdf::ElementPtr /* _sdf * /) 
t 

//Get the world nane. 
this-» model = parent; 
this 一 > world = this -> model -> GetWorld(); 


//Subscribe to Hydra updates by registering OnHydra() callback. 
this-» node = transport: :NodePtr(new transport: : Node( ) ) ; 
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this—>node—>Init(this—>world—>GetName());this—> hydraSub = this 一 >node 一 > 
Subscribe(" — /hydra", &HydraDemoPlugin::OnHydra, this); 


//Listen to the update event. This event is broadcast every 
//simulation iteration. 
this- > updateConnection = event: :Events::ConnectWorldUpdateBegin( 
boost::bind(&HydraDemoPlugin::Update, this, 1)); 
} 
HU PH P M M HP HH P M M 
voidHydraDemoPlugin::Update(const common::UpdateInfo& / * _info » /) 
t 
boost::mutex::scoped lock lock(this 一 > msgMutex) ; 


//Return if we don't have messages yet 
if (! this -> hydraMsgPtr) 
return; 


//Read the value of the right joystick. 
doublejoyX = this -> hydraMsgPtr - » right(). joy x(); 
doublejoyY = this -> hydraMsgPtr - > right(). joy y(); 


//Move the sphere. 
this 一 > model - > SetLinearVel(math::Vector3( - joyX * 0.2, joyY * 0.2, 0)); 


//Remove the message that has been processed. 
this - > hydraMsgPtr. reset() ; 
) 


在 启动 Gazebo 之 前 ,需要 将 模型 插件 包含 在 world 文件 中 。 这 里 是 部 分 内 容 里 一 个 完 
整 的 World 文件 : 
<?xml version= "1.0" ?> 


< sdf version= "1.4"> 
<world name = "default"> 


a Aod plane => 
< include> 
<uri> model://ground plane </uri> 
</include> 


<! -- Aglobal light source ——> 
<include> 
<uri>model://sun</uri> 
</include> 


第 


<! —— Load the plugin for Razer Hydra -一 > 
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<plugin name = "hydra" filename = "libHydraPlugin. so"> 


« pivot» 0.04 0 0 </pivot > 
«grab» 0.12 0 0 «/grab» 
</plugin > 


<! — A sphere controlled by Hydra 一 一 > 
< model nane = "sphere" 
<pose>000000</pose> 

< link name = "link" 

X collision name = "collision"? 
< geonetry > 

< sphere> 
<radius>0.5</radius> 
</sphere> 

</geometry> 

</collision> 

< visual name = "visual" 

< geometry > 

< sphere> 

< radius > 0.5 </radius > 

</ sphere > 

</geometry > 

</visual > 

</link> 


< plugin name = 'sphere_ctroller' filename = 'libHydraDemoPlugin. so'></plugin > 


</model > 


</world> 
</sdf > 


可 以 运行 Gazebo 并 使 用 Hydra 的 右 操 纵 杆 移动 球体 了 。 不 要 忘记 插入 Hydra, 然 后 : 


gazebo worlds/hydra_demo. world 


2. GUI 覆盖 


Gazebo GUI 覆盖 可 以 被 认为 是 位 于 泻 染 窗口 顶部 的 透明 2D 层 。QT 小 部 件 可 以 通过 
插件 界面 添加 到 此 层 。 用 户 可 以 通过 单 击 Gazebo 主 菜单 栏 View 命令 ,选择 GUI Overlays 


上 显示 或 隐藏 所 有 GUI 覆盖 层 。 下 面 介绍 如 何 创 到 
建 自 定义 界面 。 


E 和 使 用 GUI 覆盖 插件 来 为 Gazebo 创 


两 个 示例 将 用 于 演示 GUI 又 加 功能 。 第 一 个 示例 创建 一 个 生成 球体 的 按钮 ,第 二 个 示 
例 显示 当前 的 仿真 时 间 。 这 两 个 例子 展示 如 何 发 送 数据 到 Gazebo 和 从 Gazebo 接收 数据 。 
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D 示例 1: 产生 球 的 模型 
步骤 如 下 。 
安装 开发 debian, 输 入 以 下 命令 : 


sudo apt - get install libgazebo7 - dev 


从 创建 工作 目录 开始 ,输入 以 下 命令 : 


mkdir —/gazebo gui spawn 
cd —/gazebo gui spawn 


下 载 GUI 覆盖 插件 的 源 代 码 , 输 入 以 下 命令 : 


wget https://bitbucket. org/osrf/gazebo/raw/gazebo7/examples/plugins/gui overlay plugin _ 


spawn/GUIExampleSpawnWidget. hh 
wget https://bitbucket. org/osrf/gazebo/raw/gazebo7/examples/plugins/gui overlay plugin _ 


spawn/GUIExampleSpawnWidget. cc 
wget https://bitbucket. org/osrf/gazebo/raw/gazebo7/examples/plugins/gui overlay plugin _ 


spawn/CMakeLists. txt 


查看 头 文件 : 
geditGUIExampleSpawnWidget. hh 

GUI 覆盖 插件 必须 从 GUIPlugin 类 继承 ,并 使 用 Qt 的 Q_OBJECT K: 
class GAZEBO VISIBLE GUIExampleSpawnWidget : public GUIPlugin 


{ 
Q_OBJECT 


插件 的 其 余部 分 可 能 包含 使 插件 满足 用 户 的 需要 的 任何 代码 。 本 示例 使 用 QT diit De 
收 按钮 如 下 : 


///\brief Callback trigged when the button is pressed. 
protected slots: void OnButton() 


我 们 还 将 使 用 Gazebo 的 factory 功能 将 SDF 产生 的 消息 发 送 到 gzserver: 


// /Nbrief Node used to establish communication with gzserver. 
private: transport: :NodePtr node; 


// /Nbrief Publisher of factory messages. 
private: transport: :PublisherPtrfactoryPub; 
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查看 源 文件 ,输入 以 下 命令 : 


gedit GUIExampleSpawnWidget. cc 


此 文件 中 的 构造 函数 使 用 QT 创建 一 个 按钮 并 将 其 放 到 OnButton 回调 : 


//Create a push button, and connect it to the OnButton function 
QPushButton * button = new QPushButton(tr("Spawn Sphere" )); 
connect(button, SIGNAL(clicked()), this, SLOT(OnButton())); 


构造 函数 还 连接 到 Gazebo 的 传输 机 制 ,并 创建 一 个 factory publisher: 


//Create a node for transportation 
this 一 > node = transport: :NodePtr(new transport: : Node( ) ) ; 
this -» node- » Init(); 
this 一 > factoryPub = this -> node- > Advertise < msgs: :Factory>("~ /factory"); 


OnButton 可 以 回调 创建 一 个 新 的 球面 SDF 字符 串 : 


std: :ostringstreamnewModelStr; 

newModelStr << "< sdf version = '" << SDF VERSION << "'>" 
<< msgs: :ModelToSDF(model) -> ToString("") 

<< "«/sd£ >"; 


并 将 字符 串 发 送 到 Gazebo: 


msgs: :Factorymsg; 

msg.set sdf(newModelStr.str()); 
this -» factoryPub -> Publish(msg); 
) 


编译 插件 ,输入 以 下 命令 : 


cd —/gazebo gui spawn 
mkdir build 

cd build 

cmake ../ 

make 


现在 我 们 需要 确保 Gazebo 可 以 找到 插件 。 用 户 可 以 通过 将 build 目录 附加 到 
GAZEBO_PLUGIN_PATH 环境 变量 来 实现 : 


cd —/gazebo gui spawn/build 
export GAZEBO PLUGIN PRTH = 'pwd': $ GAZEBO PLUGIN PATH 
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注意 ; 上 面 的 命令 只 适用 于 当前 shell。 为 了 确保 插件 在 打开 新 终端 时 可 以 正常 工作 ， 
请 将 插件 安装 到 公共 搜索 路 径 中 ,例如 /usr/local/lib, 或 者 GAZEBO_PLUGIN_PATH X 
中 指定 的 路 径 。 

我 们 还 需要 让 Gazebo 加 载 overlay 插件 。 有 两 种 方法 来 实现 这 一 点 。 

(D SDFworld 文件 : 修改 worldworldSDF 文件 以 包含 GUI 插件 。 例 如 : 


<?xml version- "1.0" ?> 
X sdf version- "1.5"> 
< world name = "default" 


<gui> 
< plugin name = "sample" filename = "libgui example spawn widget. so"/> 
</gui> 


<! -- A global light source -一 > 

< include> 
<uri>model://sun</uri> 
</include> 

<! -— A ground plane 一 一 > 
<include> 

<uri> model://ground plane </uri> 
</include> 

</world> 

</sdf > 


注意 : 下 载 上 面 的 worldworld 文件 ,输入 以 下 命令 : 
cd —/gazebo gui spawn 


wget https://bitbucket. org/osrf/gazebo/raw/gazebo7/examples/plugins/gui overlay plugin _ 
spawn/spawn widget example. world 


(2) GUI INI x ff: 修改 一 /. gazebo/gui. ini 文件 ,以 便 每 次 运行 Gazebo 时 加 载 插件 ， 
输入 以 下 命令 : 


gedit —/.gazebo/gui. ini 
添加 以 下 行 ,输入 以 下 命令 : 


[overlay plugins] 
filenames = libgui example spawn widget.so 


现在 当 Gazebo 运行 时 ,一 个 按钮 应 该 出 现在 泻 染 窗口 的 左上 角 。 如 果 用 户 需 要 使 用 
GUI 插件 创建 自 定义 SDFworld 文件 ,输入 以 下 命令 : 
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或 者 如 果 用 户 修 改 一 /. gazebo/gui. ini。 最 后 运行 Gazebo, 如 图 4-55 所 示 , 单 击 按钮 生 
成 球体 。 


图 4-55 球体 生成 场景 


2) 示例 2: 显示 仿真 时 间 
步骤 如 下 。 
首先 ,创建 工作 目录 ,输入 以 下 命令 : 


mkdir —/gazebo gui time 
cd —/gazebo gui time 


下 载 GUI 覆盖 插件 的 源 代 码 , 输 入 以 下 命令 : 


wget https://bitbucket. org/osrf/gazebo/raw/gazebo7/examples/plugins/gui overlay plugin _ 
time/GUIExampleTimeWidget. hh 


wget https: //bitbucket. org/osrf/gazebo/raw/gazebo7/examples/plugins/gui overlay plugin . 
time/GUIExampleTimeWidget.cc 


wget https: //bitbucket. org/osrf/gazebo/raw/gazebo7/examples/plugins/gui overlay plugin | 
time/CMakeLists. txt 


看 看 头 文件 ,输入 以 下 命令 : 


geditGUIExampleTimeWidget. hh 
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和 第 一 个 例子 一 样 ,这 个 插件 继承 自 GUIPlugin 类 ,并 且 使 用 Qt 的 Q OBJECT 宏 。 
class GAZEBO VISIBLE GUIExampleTimeWidget : public GUIPlugin 


{ 
Q OBJECT 


我 们 使 用 SetSimTime 信号 作为 线程 安全 机 制 来 更 新 显示 的 仿真 时 间 : 
///\brief A signal used to set the sim tine line edit. 


///Nparam[in] string String representation of sim time. 
signals: void SetSimTime(QString string); 


一 个 OnStats 回调 用 于 从 Gazebo 接收 信息 : 
// /Nbrief Callback that received world statistics messages. 


// /Nparam[in] msg World statistics message that is received. 
protected: void OnStats(ConstWorldStatisticsPtr& msg); 


我 们 还 将 使 用 Gazebo 的 传输 机 制 来 接收 来 自 Gazebo 的 消息 : 


// /Nbrief Node used to establish communication with gzserver. 
private: transport: :NodePtr node; 


// /Nbrief Subscriber to world statistics messages. 
private: transport: :SubscriberPtrstatsSub; 


看 看 源 文件 ,输入 以 下 命令 : 


gedit GUIExampleTimeWidget. cc 


在 构造 函数 中 ,创建 一 个 QLabel 来 显示 时 间 . 并 将 其 连接 到 SetSimeTime 信号。 


//Create a time label 
QLabel * timelabel = new QLabel(tr("00:00:00.00")); 


//Add the label to the frame's layout 
frameLayout 一 > addWidget(label); 
frameLayout - > addWidget(timeLabel); 
connect(this, SIGNAL(SetSimTime(QString)), 
timeLabel, SLOT(setText(QString)), Qt::QueuedConnection); 


将 构造 函数 连接 到 Gazebo ff] — / world stats; 


//Create a node for transportation 
this-»node = transport: :NodePtr(new transport: :Node( )); 
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this—>node—>Init("default"); 
this—> statsSub = this ->node 一 > Subscribe("~/world stats", 
&GUIExampleTimeWidget::OnStats, this); 


当 接收 到 消息 时 ,OnStats 调用 该 函数 并 更 新 显示 的 时 间 : 


voidGUIExampleTimeWidget::OnStats(ConstWorldStatisticsPtr& msg) 


ü 
this - » SetSimTime(QString::fromStdString( 
this—> FormatTime( msg-» sim time()))); 


按照 与 上 一 个 部 分 内 容 相 同 的 步骤 编译 捅 件 , 告 诉 Gazebo 在 哪里 找到 它 , 并 通过 gui 
.ini SDFworld 文件 加 载 它 。 
用 户 可 以 按 如 下 所 示 添 加 两 个 插件 ,输入 以 下 命令 : 


gedit —/.gazebo/gui. ini 
Tf [overlay plugins ]iB 4 E vi Jy : 


[overlay plugins] 
filenames - libgui example spawn widget.so:libgui example time widget.so 


这 将 加 载 前 一 个 示例 的 spawn 插件 和 本 示例 中 的 时 间 插 件 。 
运行 Gazebo 时 ,按钮 右 侧 的 新 文本 框 应 显示 仿真 时 间 , 见 图 4-56。 


Sim Time: 00:00:12 


图 4-56 显示 仿真 时 间 的 窗口 
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4.8.3 连接 到 Player 


Player 是 一 个 机 器 人 控制 框架 ,下 面 介 绍 如 何 将 position2d Player 接口 连接 到 
Gazebo, 


创建 工作 目录 ,输入 以 下 命令 : 


cd; mkdir gazebo position2d; cd gazebo position2d 


将 下 面 的 配置 脚本 复制 到 position2d. cfg 文件 中 : 


driver 
( 
nane "gazebo" 
provides ["simulation:0"] 
plugin "libgazebo player" 
* The name of a running Gazebo world, specified ina .world file 
world name "default" 


) 
driver 
( 
name "gazebo" 
provides ["position2d:0"] 
# This name must match the name of a model in the "default" world 


model name "pioneer2dx" 


) 

输入 以 下 命令 运行 Gazebo: 
gazebo worlds/pioneer2dx. world 
输入 以 下 命令 运行 Player: 


player position2d. cfg 


输入 以 下 命令 运行 playerv: 
playerv 


完成 以 上 的 步骤 后 ,现在 用 户 可 以 使 用 playerv 驱动 pioneer2dx. 
接 下 来 介绍 如 何 将 相机 播放 器 界面 连接 到 Gazebo, 
创建 工作 目录 ,输入 以 下 命令 : 
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cd; mkdirgazebo camera; cd gazebo camera 


将 下 面 的 配置 脚本 复制 到 camera. cfg 文件 中 : 


driver 


( 
nane "gazebo" 
provides ["simulation:0"] 
plugin "libgazebo player" 
# The name of a running Gazebo world, specified ina .world file 
world name "default" 
) 
driver 
( 
name "gazebo" 
provides ["camera:0"] 
# This name must match a scoped name of a sensor in the "default" world 
* The format of a scoped name is "model name::link name::sensor name" 


camera name "pioneer2dx::camera::link::camera" 


) 
输入 以 下 命令 运行 Gazebo: 
gazebo worlds/pioneer2dx camera.world 
输入 以 下 命令 运行 Player: 
playercamera. cfg 
输入 以 下 的 命令 运行 playerv: 
playerv 
完成 以 上 的 步骤 后 ,现在 用 户 可 以 在 playerv 中 可 看 到 相机 。 
本 章 小 结 
本 章 介绍 了 机 器 人 仿真 软件 Gazebo 的 应 用 ,重点 部 分 在 于 如 何 使 用 Gazebo 创建 机 器 


人 和 场景 文件 的 创建 。 在 Gazebo 的 学 习 中 ,应 该 注意 与 V-REP 之 间 的 差别 ,以 及 如 何 与 
机 器 人 操作 系统 进行 结合 。 
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UM OpenRAVE 在 机 器 人 
仿真 中 的 应 用 


5.1 OpenRAVE 简介 


5.1.1 OpenRAVE 的 应 用 


OpenRAVE( 英 文 全 称 为 Open Robotics Automation Virtual Environment) 是 一 款 开 
源 的 机 器 人 仿真 软件 。OpenRAVE 提供 了 机 器 人 的 测试 环境 , 它 的 主要 功能 是 运动 规划 运 
动 学 和 几何 信息 的 模拟 和 分 析 。 在 应 用 方面 ,OpenRAVE 主要 用 于 开发 和 部 署 机 器 人 的 运 
动 规划 算法 ,而 这 些 算法 能 够 应 用 于 实际 中 的 机 器 人 。 由 于 OpenRAVE 具备 有 独立 运行 
的 性 质 , 这 些 算 法 可 以 很 容易 地 集成 到 现 有 的 机 器 人 系统 。 它 为 机 器 人 开发 者 和 机 器 人 提 
供 了 许多 命令 行 工具 ,核心 运行 时 足够 小 ,因此 可 用 于 内 部 控制 器 和 更 大 的 框架 。 

OpenRAVE 是 一 个 开放 源码 跨 平 台 软 件 架构 , 即 开放 的 机 器 人 和 动画 虚拟 环境 。 
OpenRAVE 针对 真实 世界 自动 机 器 人 应 用 程序 ,包括 3-D 模拟 .可 视 化 .规划 ,脚本 和 控制 
的 无 缝 集成 。 它 的 插件 架构 允许 用 户 轻 松 地 编写 自 定义 的 控制 器 和 进行 扩展 功能 。 通 过 使 
用 OpenRAVE 插件 ,任何 设计 的 算法 、 机 器 人 控制 器 或 感 测 子 系统 都 可 以 在 运行 时 进行 分 
布 和 动态 加 载 , 从 而 使 开发 人 员 免 于 使 用 单 片 代 码 库 。 这 样 OpenRAVE 的 用 户 可 以 专注 
于 问题 的 规划 和 脚本 方面 的 开发 ,而 无 须 明确 管理 机 器 人 运动 学 和 动力 学 .碰撞 检测 .世界 
更 新 和 机 器 人 控制 的 细节 OpenRAVE 架构 也 提供 了 一 个 灵活 的 接口 ,可 以 与 其 他 流行 的 
机 器 人 软件 包 ( 如 Player 和 ROS) 结 合 使 用 ,因为 它 专 注 于 自动 运动 规划 和 高 级 脚本 ,而 不 
是 低级 控制 和 信息 协议 。OpenRAVE 还 支持 强大 的 网 络 脚本 环境 ,使 得 在 运行 时 控制 和 监 
视 机 器 人 以 及 更 改 执行 流程 变 得 更 加 简单 。 开 放 组 件 架 构 的 一 个 关键 优势 是 它们 使 机 器 人 
研究 团体 能 够 轻松 地 共享 和 比较 算法 。 

下 文 将 主要 对 OpenRA VE 的 原理 以 及 运用 进行 简单 介绍 。 


5.1.2 OpenRAVE 的 特性 


OpenRAVE 具有 许多 功能 用 于 分 析 机 器 人 场景 的 几何 结构 ,然后 把 它们 用 于 在 整个 工 
中 使 机 器 人 运动 。 


* 
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OpenRAVE 在 两 方面 上 具有 良好 的 应 用 : 

CD 对 于 每 个 机 器 人 ,使 用 IKFast 能 够 针对 某 种 机 器 人 的 结构 专门 地 生成 逆 运 动 学 程 
序 。 这 允许 所 有 奇 点 配置 和 除 以 零 条 件 的 处 理 。 而 且 , 这 种 处 理 的 速度 特别 快 ,生成 大 多 数 
解决 方法 只 需 运行 ps. 

(2) 可 以 很 容易 地 结合 多 个 约束 条 件 , 例 如 避免 碰撞 、 把 握 对 象 , 保 持 传 感 器 能 见 度 。 
然后 ,在 这 些 约 束 条 件 下 把 一 个 机 器 人 的 初始 和 目标 配置 连接 在 一 起 。 

OpenRAVE 是 一 个 开放 机 器 人 和 3-D 动画 虚拟 环境 。 相 比 于 其 他 仿真 工具 ， 
OpenRA VE 具有 它 的 独特 优势 ; 

(1) 能 用 于 机 器 人 的 实时 控制 和 执行 监控 的 集成 设计 。 

(2) 提供 运动 学 操作 和 物理 模拟 的 核心 功能 。 

(3) 有 允许 诸如 Octave 和 MATLAB 之 类 的 解释 性 脚本 语言 与 其 进行 交互 的 网 络 协 
议 ( 当 然 还 支持 其 他 脚本 语言 ,例如 Python 和 Perl 是 计划 开发 的 ) 。 

(4) 内 置 核心 工具 和 插件 界面 ,用 于 机 器 人 的 操作 规划 和 抓 取 。 

C5) 标准 插件 ,允许 测试 不 同 的 规划 算法 和 传 感 系统 ,而 只 需 做 最 少 的 代码 修改 。 

OpenRAVE 架构 模块 化 了 机 器 人 系统 的 执行 和 计划 层 , 使 自主 系统 的 开发 变 得 更 容 
易 ,组 件 变 得 更 可 重用 于 其 他 项 目 。 一 个 基本 做 法 是 在 特定 组 件 的 实现 与 从 其 他 组 件 的 使 
用 这 个 特定 组 件 之 间 创 建 一 个 接口 层 。 许 多 以 前 的 架构 已 经 为 基本 级 别 组 件 做 到 这 一 点 。 


5.1.3 OpenRAVE 的 下 载 与 安装 


OpenRAVE 提供 了 可 以 用 于 Ubuntu 和 Windows 的 版 本 。 下 面 介 绍 在 Ubuntu 上 安 
装 OpenRAVE 的 步骤 。 

1. 安装 依赖 库 

首先 保证 已 安装 了 下 面 的 项 目 ,从 命令 行 输入 : 

01. sudo apt - get install cmake g++ git qt4 - dev - tools zlib- bin 


02. sudo apt - get install ipython python - dev python - h5py python - numpy python - scipy python 
- sympy 


安装 依赖 库 : 

01. sudo apt - get install libassimp - dev libavcodec - dev libavformat - dev libavformat — dev 
libboost -all - dev libboost - date - time - dev libbullet - dev libfaac - dev libglew — dev 
libgsml- dev liblapack — dev libmpfr - dev libode - dev libogg - dev libopenscenegraph - dev 


libpcre3 - dev libpcrecpp0 libqhull - dev libqt4 - dev libsoqt - dev - common libsoqt4 - dev 
libswscale - dev libswscale - dev libvorbis - dev libx264 - dev libxml2 - dev libxvidcore - dev 


唯一 从 OpenRA VE ppa 上 可 以 安装 的 包 是 collada-dom: 


01. sudo add — apt - repository ppa: OpenRAVE/release 
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02. sudo sh — c 'echo "deb— src http://ppa. launchpad. net/OpenRAVE/release/ubuntu 'lsb release 
— cs' nain" >> /etc/apt/sources. list.d/OpenRAVE - release - 'lsb release - cs'.list' 

03. sudo apt - get update 

04. sudo apt — get install collada - dom - dev 


2. 从 源 文 件 安装 

从 命令 行 输入 : 

01. git clone https://github. com/rdiankov/OpenRAVE. git 
编译 : 

01. cd OpenRAVE 

02. mkdir build 

03. cd build 

04.cmake .. 


05. make 
06. sudo make install 


将 OpenRAVE 的 bash 文件 添加 到 系统 环境 : 

01. vin . bashrc 

最 后 一 行 加 入 以 下 路 径 代 码 : 

01. view plain copy print? source /usr/local/share/OpenRAVE - 0. 9/OpenRAVE. bash 
运行 实例 如 下 : 


01.html] view plain copy print? OpenRAVEO.9.py —- example Hanoi 


5.2 OpenRAVE 概观 


5.2.1 OpenRAVE 基本 架构 


OpenRAVE 的 4 个 主要 部 件 如 图 5-1 所 示 。 

1. 核心 层 

它 的 核心 是 由 一 组 定义 了 插件 如 何 共享 信息 的 基本 接口 类 组 成 ,并 提供 了 一 个 环境 接 
口 , 来 维持 一 个 主要 状态 ,作为 通过 OpenRAVE 提供 的 所 有 功能 的 关卡 , 它 是 全 局 
OpenRA VE 状态 管理 加 载 的 插件 ,有 多 个 独立 空间 和 日 志 记录 。 同 时 ,这 个 环境 将 碰撞 检 
查 器 、 观 察 器 ,物理 引擎 、 运 动 学 世界 及 其 所 有 接口 组 合成 一 致 的 机 器 人 世界 状态 。 
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图 5-1 OpenRAVE 的 基本 架构 


2. 插件 层 

OpenRAVE 设计 为 基于 插件 的 架构 ,其 中 插件 提供 了 动态 加 载 到 环境 中 的 基本 接口 类 
的 实现 。 插 件 可 以 与 其 他 机 器 人 库 连 接 , 允 许 OpenRAVE 扩展 其 功能 ,或 者 可 以 向 另 一 个 
机 器 人 系统 提供 OpenRAVE 服务 。 在 它 的 启动 过 程 中 , OpenRAVE 将 会 解析 
OPENRAVE_PLUGINS 环境 变量 并 加 载 其 找到 的 所 有 插件 。 

3. 脚本 层 

OpenRAVE 为 Python 和 Octave/MATLAB 提供 了 脚本 环境 。Python 与 核心 层 通过 
内 存 调 用 直接 通信 ,使 得 通信 速度 变 得 极 快 。 另 一 方面 ,Octave/MATLAB 脚本 协议 通过 
TCP/IP 发 送 命令 ,一 个 插件 在 OpenRAVE 核心 端 提供 一 个 文本 服务 器 。 脚 本 人 允许 在 不 需 
要 关闭 的 前 提 下 实时 修改 环境 的 任何 方面 .使 其 成 为 测试 新 算法 的 理想 选择 。Python 脚本 
是 如 此 强大 ,使 得 大 多 数 的 OpenRAVE 示例 和 演示 代码 通过 它 提供 。 事 实 上 ,用 户 应 该 将 
脚本 语言 看 作 整 个 系统 的 一 个 组 成 部 分 ,而 不 是 作为 C++ API 的 替代 。 

4. 机 器 人 数据 库 层 

实现 规划 知识 库 ,并 为 其 访问 和 生成 参数 提供 简单 的 界面 。 数 据 库 本 身 主 要 包括 机 器 
人 和 任务 的 运动 学 、 准 静态 .动态 和 几何 分 析 。 如 果 机 器 人 被 正确 定义 ,那么 所 有 这 些 功 能 
都 应 该 能 被 直接 调用 。 

所 有 基本 规划 器 和 模块 应 适用 于 任何 的 机 器 人 结构 。 与 其 他 规划 包 相 比 ,OpenRAVE 
的 一 个 优点 是 能 够 在 OpenRAVE 中 应 用 算法 到 任何 机 器 人 ,而 只 需 很 少 的 修改 。 最 近 , 已 
经 引入 了 允许 计算 (如 凸 包 分 解 , 抓 取 集 ,可 达 性 图 ,分 析 逆 运动 学 等 ) 属 性 的 规划 数据 库 结 
构 。 如 果 机 器 人 被 正确 定义 ,那么 所 有 这 些 功能 都 应 该 能 够 被 直接 调用 。 

主要 的 API 是 Dawes 等 人 使 用 Boost C++ 库 在 C++ 中 编码 的 ,并 且 作为 低级 管理 和 存 
储 结 构 的 真正 坚实 的 基础 。Boost 的 共享 指针 的 风格 允许 在 大 量 多 线程 环境 中 安全 地 引用 
对 象 指针 。 共 享 指针 还 允许 将 句柄 和 接口 传递 给 用 户 ,而 不 必 担 心 用 户 调用 无 效 对 象 或 纯 
载 共享 对 象 。 此 外 ,OpenRAVE 使 用 通常 在 更 高 级 语言 中 看 到 的 函数 和 其 他 抽象 对 象 来 指 
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定 用 于 采样 分 布 .事件 回调 \ 设 置 机 器 人 配置 状态 等 的 函数 指针 。 启 用 Boost 的 设计 使 得 
C++ API 真正 安全 可 靠 , 同 时 减少 了 用 户 在 结尾 处 进行 统计 的 麻烦 。 此 外 , 它 允 许 资源 获 
取 初 始 化 (RAID 设 计 模式 被 完全 利用 ,允许 用 户 忽 略 多 线程 资源 管理 的 复杂 性 。 

如 图 5-2 所 示 ,客户 端 与 服务 端 模 型 允许 任何 脚本 或 GUI 实例 同时 与 多 个 运行 中 的 
OpenRAVE 主要 例 程 通信 ; 并 且 主 要 例 程 可 以 彼此 通信 ,从 而 周期 性 地 同步 它们 的 世界 的 
内 部 视图 。 


(Matlab) (Matlab) 
脚本 实例 脚本 实例 


OpenRAVE 
实例 


图 5-2 脚本 实例 与 OpenRAVE 实例 之 间 的 通信 


(Matlab) 
脚本 实例 


网 络 层 


OpenRAVE 
实例 


5.2.2. 关于 OpenRAVE 中 的 一 些 说 明 


1. 环境 概念 

所 有 OpenRAVE 的 服务 都 是 通过 所 在 的 环境 提供 的 。 例 如 ,通过 RaveCreatePlanner() 请 
求 称 为 “BiRRT” 的 计划 程序 接口 。 环 境 支 持 : 

。 管 理 与 沟通 的 插件 ; 

。 磁 撞 检测 ; 

。 加 载 场景 与 对 象 ; 

。 管理 对 象 和 三 角 测 量 ; 

e drawing 和 ploting 。 

每 当 写 人 或 读 取 环 境 中 的 对 象 时 ,用 户 必 须 锁 定 mutex 环境 : mutexGetMutex()。 这 
防止 了 任何 其 他 进程 在 用 户 工 作 时 修改 环境 。 因 为 环境 使 用 递归 互 斥 , 它 允 许 互 斥 锁 在 同 
一 线程 内 根据 需要 被 锁定 多 次 。 这 样 就 允许 所 有 需要 锁定 的 环境 函数 始终 保证 互 斥 锁 被 锁 
定 ,而 不 管用 户 是 否 锁定 了 互 斥 锁 。( 注 意 , 这 仅 适 用 于 环境 函数 ,而 不 适用 于 接口 函数 .) 

1) 锁定 

因为 OpenRAVE 是 一 个 高 度 多 线程 的 环境 ,所 以 可 以 同时 访问 诸如 主体 和 加 载 的 接 
口 的 环境 状态 。 为 了 安全 地 处 于 写 或 读 状态 ,用 户 必须 锁定 环境 ,这 防止 任何 其 他 进程 在 用 
户 工作 时 修改 环境 。 通 过 使 用 递归 锁 , 它 允许 锁 在 同一 线程 内 当 变 换 状 态 改变 函数 调用 另 
一 状态 改变 函数 时 根据 需要 被 锁定 许多 次 ,从 而 极 大 地 减少 了 锁 管 理 。 此 安全 措施 帮助 用 
户 在 调用 全 局 级 环境 功能 (例如 创建 新 实体 或 加 载 场景 ) 时 始终 保证 环境 被 锁定 ,而 不 管用 
户 是 否 记 得 锁定 。 

2) 仿真 线程 

每 个 环境 都 有 一 个 内 部 时 间 和 一 个 直接 连接 到 物理 引擎 的 仿真 线程 。 线 程 总 是 在 后 台 
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运行 ,并 且 通 过 物理 引擎 和 所 有 启用 仿真 的 接口 的 小 增 量 来 周期 性 地 对 仿真 时 间 进 行 处 理 。 
默认 情况 下 ,线程 总 是 在 运行 ,并 且 总 是 潜在 地 修改 环境 状态 。 因 此 ,只 要 使 用 内 部 状态 ,如 
通过 设置 联合 值 或 链接 转换 修改 主体 ,用 户 总 是 需要 明确 地 锁定 环境 。 如 有 不 慎 , 控 制 器 或 
物理 引擎 将 覆盖 它们 。 默 认 情 况 下 ,仿真 线程 仅 根据 其 控制 器 输入 来 设置 对 象 位 置 ,但 可 以 
通过 附加 物理 引擎 集成 速度 、 加 速度 、 力 和 力矩 。 

模拟 线程 最 初 令 人 感觉 很 麻烦 ,但 是 它 将 机 器 人 控制 划分 为 控制 输入 计算 和 执行 ,大 大 
有 助 于 用 户 只 关注 给 机 器 人 的 进 给 命令 ,而 不 必 担 心 模拟 循环 。 它 还 允许 环境 更 新 发 生 在 
一 个 离散 时 间 轴 上 。 

3) 复制 

OpenRAVE 的 优势 之 一 是 允许 多 个 环境 在 同一 个 进程 中 同时 工作 。 环 境 复制 允许 
OpenRAVE 通过 在 它们 之 上 管理 多 个 环境 并 运行 同步 计划 程序 来 实现 真正 的 并 行 运 行 。 
因为 复制 和 原始 环境 之 间 没 有 共享 状态 ,所 以 不 可 能 使 用 从 另 一 个 环境 中 的 一 个 环境 创建 
的 接口 。 例 如 ,如 果 在 一 个 环境 中 创建 计划 程序 , 则 它 只 应 由 该 环境 中 的 对 象 使 用 。 能 设置 
计划 器 来 计划 属于 不 同 环境 的 对 象 。 这 是 因为 将 锁定 环境 ,并 期 望 它 控 制 的 对 象 完全 在 其 
控制 之 下 。 

创建 复制 很 简单 ,在 C++ 中 只 需 键入 : 


EnvironmentBasePtrpNewEnvironment = GetEnv() ->CloneSelf(Clone Bodies) 


用 以 创建 复制 所 有 现 有 主体 ( 带 附 件 和 抓 取 主体 ) 及 其 当前 状态 的 复制 。 基 本 上 ,复制 
可 以 执行 和 使 用 原始 环境 完成 的 任何 操作 。 

因为 环境 状态 非常 复杂 ,所 以 复制 过 程 可 以 控制 其 中 有 多 少 传输 到 新 复制 。 例 如 ,可 以 
复制 所 有 现 有 的 机 构 和 机 器 人 ,也 可 以 复制 它们 附 接 的 控制 器 ,可 以 复制 它们 附 接 的 查看 
器 ,也 可 以 复制 碰撞 检查 器 状态 ,并 且 可 以 复制 仿真 状态 。 基 本 上 ,复制 应 该 能 够 执行 可 以 
在 原始 环境 中 完成 的 任何 操作 ,而 无 须 对 输入 参数 进行 任何 修改 。 

当 复 制 真实 的 机 器 人 时 ,OpenRAVE 复制 提供 的 一 个 非常 重要 的 特性 是 能 够 保持 传 感 
器 不 断 更 新 新 信息 的 环境 的 实时 视图 。 当 计划 器 被 实例 化 时 , 它 可 以 制作 其 可 以 独占 控制 
而 不 干扰 更 新 操作 的 环境 的 副本 。 此 外 ,现实 世界 环境 可 能 具有 连接 到 真实 机 器 人 的 机 器 
人 控制 器 ,复制 具有 给 出 设置 仿真 控制 器 的 能 力 , 保 证 机 器 人 在 规划 时 的 安全 。 来 自 复制 环 
境 的 命令 不 会 意外 地 向 真实 机 器 人 发 送 命令 。 

4) 验证 插件 

每 个 插件 需要 导出 几 个 函数 来 通知 核心 它 有 什么 接口 和 实例 化 接口 。 当 插件 首次 加 载 
时 , 它 由 环境 验证 ,并 查询 其 接口 信息 ,以便 核 心 可 以 注册 名 称 。 

在 验证 过 程 中 有 许多 机 制 来 防止 旧 的 插件 被 核 加 载 。OpenRAVE 经 常 更 新 ,所 有 用 户 
插件 不 一 定 在 OpenRAVE API 更 改 时 重新 编译 。 因 此 ,我 们 会 遇 到 很 多 情况 , 当 插件 导出 
正确 的 函数 时 , 却 没有 实现 正确 的 API。 使 用 不 匹配 编译 的 插件 使 用 接口 API 可 能 导致 非 
常 难以 调试 的 意外 崩溃 ,因此 绝对 需要 检测 此 情况 。 一 个 可 能 的 解决 方案 是 向 API 添加 版 
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本 号 ,以 在 接口 从 插件 返回 到 环境 之 前 执行 检查 ,但 是 这 种 方法 很 脆弱 。 它 强制 跟踪 每 个 接 
口 的 版 本 号 以 及 全 局 版 本 号 。 此 外 ,即使 是 一 个 小 的 变化 ,每 个 开发 人 员 都 必须 记 住 增加 版 
本 ,但 这 很 容易 被 遗忘 ,并 在 后 来 导致 严重 的 错误 。 

我 们 通过 计算 接口 函数 和 成 员 的 唯一 hash 来 解决 接口 验证 问题 ,这 是 通过 C++ lexer 
运行 每 个 接口 ,收集 影响 C++ 代码 结构 的 标志 ,然后 创建 一 个 128 位 的 唯一 MD5hash。 我 
们 为 每 个 接口 定义 和 环境 创建 一 个 hash。 散 列 被 硬 编码 到 C++ 头 文件 中 ,并 且 可 以 通过 两 
种 方法 来 查询 : 返回 调用 函数 的 程序 的 静态 函数 的 散 列 ,以 及 返回 编译 接口 的 虚 函 数 的 散 
列 。 只 有 当 其 虚拟 hash 等 同 于 核心 环境 的 静态 hash 时 ,接口 才 有 效 。 为 了 正确 加 载 插件 ， 
首先 环境 hash 必须 匹配 。 检 查 单个 接口 ,只 有 匹配 的 接口 才 会 返回 到 核心 ,并 从 那里 分 派 
到 其 他 插件 。 这 种 一 致 性 检查 ,确保 过 期 插件 将 永远 不 会 被 加 载 。 

5) 并 行 执行 

能 够 在 多 线程 中 执行 计划 程序 对 于 需要 速度 和 解决 方案 质量 的 应 用 程序 非常 重要 。 由 
于 解决 方案 质量 和 计算 时 间 之 间 总 是 存在 折 中 ,一些 应 用 程序 (如 工业 机 器 人 ) 需 要 最 快 .最 
平滑 地 抵达 其 目的 地 。 幸 运 的 是 ,环境 复制 允许 规划 者 为 每 个 线程 创建 一 个 独立 的 环境 ,这 
使 得 他 们 可 以 调用 每 个 相应 线程 中 的 运动 学 和 碰撞 函数 ,而 不 用 担心 数据 损坏 。 在 多 线程 
环境 中 生长 RRT 树 只 需要 维护 kd-tree 结构 的 一 个 副本 。 查 询 操作 主要 使 用 配置 空间 上 
的 欧式 距离 ,所 以 这 样 会 更 快 。 此 外 ,添加 新 点 需要 O(log) 时 间 , 当 然 与 冲突 检查 相 比 , 它 
不 应 该 是 搜索 过 程 中 的 瓶颈 。 最 后 ,环境 锁定 允许 线程 获得 对 环境 的 独占 访问 。 最 好 的 方 
法 是 属于 或 添加 到 环境 的 任何 接口 在 调用 其 任何 方法 之 前 都 需要 进行 环境 锁定 。 

2. 双 模 拟 / 控 制 性 质 

OpenRAVE 可 以 用 作 模 拟 、 控 制 器 ,或 同时 用 作 两 者 。 有 几 点 需要 注意 : 

CD 它 可 以 通过 附加 物理 引擎 来 设置 扭矩 到 关节 和 施加 力 到 连 杆 来 用 作 模拟 器 。 

(2) 物理 引擎 直接 反映 OpenRAVE 的 内 部 状态 。 

(3) 可 以 设置 每 个 时 间 段 都 向 物理 引擎 设置 扭矩 .速度 、 位 置 的 控制 器 。 如 果 模 拟 设置 
为 true( 默 认 ) ,物理 仿真 的 时 间 周 期 会 在 内 部 “OpenRAVE 线程 "中 不 断 调用 。 

(4) 默认 物理 引擎 不 接触 OpenRA VE 状态 ,也 不 模拟 速度 或 动力 学 。 

(5) 默认 控制 器 只 是 在 指定 的 时 间 设 置 位 置 。 

这 就 是 为 什么 每 当 内 部 为 OpenRAVE 状态 ,如 设置 联合 值 或 链接 转换 (例如 在 规划 
者 ) 时 ,用 户 都 需要 明确 地 锁定 环境 互 斥 ,否则 ,控制 器 或 物理 引擎 将 覆盖 它们 。 

3. 异常 处 理 

通过 使 用 C++ 标准 和 Boost 库 .OpenRAVE 可 以 从 用 户 可 以 碰 到 的 几乎 所 有 错误 中 恢 
复 ,而 不 会 导致 程序 在 现场 关闭 。 无 效 指针 和 超 范围 访问 是 非常 危险 的 ,因为 它们 可 以 修改 
不 相关 的 内 存 , 这 导致 程序 在 与 问题 的 根本 原因 完全 无 关 的 地 方 崩溃 。 避 免 这 样 的 问题 一 
直 是 设计 的 最 高 优先 级 之 一 。 核 心 总 是 包含 着 来 自 插件 和 回调 的 try/catch 块 的 任何 用 户 
代码 ,这 允许 核心 正确 处 理 错误 并 通知 用 户 一 个 问题 ,而 不 会 破坏 环境 。 因 为 异常 处 理 很 
慢 , 所 以 函数 应 该 返回 错误 代码 以 及 它 应 该 抛 出 异常 的 细微 问题 处 。 在 OpenRAVE 中 ,在 
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程序 的 正常 操作 中 不 应 该 发 生 异 常 ,它们 应 该 只 是 程序 的 意外 事件 。 例 如 ,计划 程序 失败 是 
取决 于 当前 环境 的 预期 事件 ,因此 计划 程序 应 返回 具有 失败 原因 的 错误 代码 ,而 不 是 抛 出 异 
常 。 换 句 话 说 ,异常 传达 应 该 指向 代码 中 由 用 户 固定 的 位 置 的 程序 的 结构 性 错误 。 下 面 给 
出 在 OpenRAVE 会 报错 的 一 些 情况 : 

。 无效 的 插件 或 接口 hash ff; 

。 无 效 的 命令 发 送 到 接口 ; 
无 效 的 参数 传递 给 函数 ; 
无 效 指针 或 超出 范围 列表 份 被 访问 ; 
。 当 环 境 需 要 被 锁定 时 没有 被 锁定 时 ; 
。 数 学 运算 与 其 他 环境 不 一 致 ; 
不 保持 环境 命名 约束 ; 
给 出 了 无 法 识别 的 枚 举 类 型 

。 不 保持 实例 化 顺序 。 

任何 类 型 的 boost 错误 或 空 指针 访问 都 会 抛 出 一 个 OpenRAVE 异常 报错 。 这 大 大 减 
少 了 人 们 做 错误 检查 代码 的 数量 。 例 如 ,C 代码 通常 有 这 样 的 模式 : 


boolsomefun(KinBodyPtrpbody) 
{ 

if( !pbody ) 

returnfalse; 

pbody - » GetTransform(); 


或 者 
boolsonefun(KinBodyPtrpbody) 
i 1! pbody ) ; 


pbody - > GetTransform(); 


) 


如 果 这 些 检查 都 没有 做 ,代码 段 将 报错 。 然 而 ,这 些 能 够 真正 检查 混乱 的 代码 。 在 
OpenRAVE, 它 能 安全 地 通过 下 面 的 方式 跳出 : 

boolsomefun(KinBodyPtrpbody) 

{ 

pbody - > GetTransform(); 


} 
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对 于 处 理 错 误 ( 例 如 ,在 最 顶层 的 脚本 ) .可 以 这 样 : 
try ( 
saeva ood] 


l 

catch(constOpenRAVE exception& ex) { 

RAVELOG WARN("exception caught: % sWVn", ex. what()); 
if(ex.GetCode() -- ORE EnvironmentNotLocked ) { 
RAVELOG WARN("user forgot to lock environment! \n"); 
) 


) 


当 在 python 中 使 用 OpenRAVEpy 时 ,这 种 未 处 理 的 C++ 错误 会 抛 出 一 个 python 异 
常 , 它 可 以 被 安全 地 捕获 和 处 理 。 

4. 物体 结构 的 hash f& 

OpenRAVE 的 一 个 新 概念 是 创建 一 个 身体 结构 的 独特 散 列 。 每 个 机 构 都 有 一 个 在 线 
状态 ,包括 : 

。 身体 的 名 称 , 其 链接 ,其 关节 ; 

。 连 杆 变换 , 它 的 速度 和 加 速度 ; 

。 连接 体 。 

所 有 其 他 信息 独立 于 环境 ,可 以 分 为 运动 学 、 几 何 学 和 主体 的 动力 学 。 此 外 ,机 器 人 具 
有 用 于 连 杆 的 传感器 和 操纵 器 的 类 别 。 规 划 知 识 库 存储 关于 身体 和 机 器 人 的 所 有 缓存 的 信 
息 , 因 此 它 需 要 以 一 致 的 方式 索引 此 信息 。 通 过 机 器 人 名 称 索引 是 不 可 靠 的 ,因为 每 次 更 改 
主体 结构 时 都 非常 难以 提醒 用 户 更 改名 称 。 因 此 ,OpenRAVE 提供 了 序列 化 主体 的 不 同类 
别 并 创建 128 位 MD5hash 的 功能 。 规 划 知 识 库 中 的 每 个 模型 都 依赖 于 机 器 人 的 不 同类 别 。 
例如 : 

CD 道 运动 学 生成 仅 使 用 由 机 械 手 和 抓 握 坐 标 系 限定 的 机 器 人 的 子 链 的 运动 学 ; 

(2) 运动 学 可 达 性 关心 机 器 人 几何 问题 ,因为 它 隐 式 存储 自我 碰撞 的 结果 ; 

(3) 道 可 达 性 进一步 使 用 基 机 器 人 连 杆 连接 到 基 座 操纵 器 连 杆 的 链 路 ; 

(4) 抓 握 关 心目 标 体 的 几何 形状 以 及 夹具 的 运动 学 和 几何 形状 ; 

(5) 凸 分 解 只 关心 链 路 的 几何 形状 ; 

(6) 道 动力 学 只 关心 每 一 个 环节 和 运动 学 的 动态 性 能 。 

在 所 有 操作 系统 和 编译 器 中 开发 一 致 的 索引 有 几 个 挑战 ,因为 浮 点 值 在 标准 化 浮 点 值 
时 会 出 现 浮 点 错误 。 然 而 ,这 样 的 索引 的 想法 可 以 极 大 地 帮助 开发 世界 各 地 的 机 器 人 数据 
库 , 任 何人 都 可 以 使 用 。 

5. 资源 文件 格式 

OpenRAVE 定义 了 自己 的 OpenRAVE XML 格式 ,允许 实例 化 任何 OpenRAVE 接口 
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和 快速 建立 机 器 人 和 运动 结构 。 刚 体 几何 资源 几乎 可 以 由 任何 3D 文件 格式 指定 。 
例如 : 
* iv. vrml, wrl, stl, blend. 3ds. ase. obj. ply. dxf. lwo, Ixo. ac. ms3d, x. mesh 
. xml. irrmesh. irr. nff, off. raw。 这 些 文件 可 以 在 < geom > 标记 内 部 使 用 ,或 者 可 
以 直接 读 入 任何 环境 中 的 ReadRobotX 和 ReadKinBodyX 方法 来 创建 单个 事件 
机 构 。 
OpenRAVE 还 支持 关于 3D 几何 和 建 模 的 COLLADA 国际 标准 。COLLADA 通过 这 
些 OpenRAVE 机 器 人 特定 的 扩展 而 得 到 扩展 。 


5.2.3 OpenRAVE 公约 与 准则 


1. 几何 约定 

CD. 内 部 矩阵 是 按 列 顺序 的 行 主 格式 ,这 意味 着 仿 射 矩阵 表示 使 用 标准 数学 方法 。 所 
有 和 矩阵 以 列 主格 式 序 列 化 ,这 是 为 了 使 Octave/ MATLAB 在 矩阵 之 间 转 换 更 简单 。 注 意 ， 
python 使 用 行 主 矩阵 的 格式 , 当 传递 到 两 个 接口 时 需要 转 置 。 

(2) 四 元 数 ,表示 旋转 的 优选 方式 ,用 标量 值 定义 为 第 一 分 量 。 例 如 [w x y zj 或 [cos 
sin * axis]. 

(3) 姿态 是 指定 为 四 元 数 和 平移 的 仿 射 变换 。 将 它 序列 化 为 7 个 值 ,前 4 个 是 四 元 数 。 

(4) 两 个 旋转 之 间 的 距离 是 cos la e gz | ,其 中 每 个 旋转 表示 为 四 元 数 。 对 于 彼此 接 
近 的 旋转 ,这 有 时 近似 为 : min qal lg tal). 

(5) 联合 轴 旋 转 定 义 为 逆 时 针 旋 转 。 

2. 机 器 人 约定 

CD 机 器 人 的 上 方向 在 正 = 轴 上 ,前进 方向 是 正 工 轴 。 

(2) 移动 操纵 在 XY 平 面 上 进行 。 

(3) 机 器 人 的 原点 应 该 被 定义 为 使 得 其 基部 完美 地 重 又 在 z— 0 处 的 平面 上 ,并 且 当 基 
部 形成 自然 的 就 地 转弯 时 ,x 轴 为 旋转 的 中 心 轴 。 

OD 默认 环境 尺度 的 所 有 物体 、 机 器 人 应 以 米 为 单位 。 有 许多 默认 阔 值 和 参数 符合 这 
个 约定 ,而 如 果 不 遵循 它 会 导致 计算 爆炸 。 更 一 般 的 约定 是 ,应 该 选择 单位 元 ,使 得 机 器 人 
的 臂 长 最 接近 1. 

(5) 机 器 人 ,kinbody 中 的 每 个 链接 .操纵 器 传感器 .关节 都 应 该 有 一 个 名 称 , 以 区 别 于 
其 他 。 

(6) 首次 加 载 到 场景 中 时 机 器 人 的 初始 配置 不 能 处 于 自我 碰撞 状态 。 

3. 环境 公约 

添加 到 环境 中 的 每 个 机 构 应 该 有 一 个 唯一 的 名 称 。 


s.2.4 OpenRAVE 中 机 器 人 概述 
OpenRAVE 支持 用 于 指定 机 器 人 的 COLLADA 文件 格式 ,并 添加 了 其 自己 的 一 组 机 
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器 人 专用 扩展 。COLLADA 格式 可 用 于 指定 所 有 机 器 人 和 场景 相关 信息 。COLLADA 文 
件 保 存 为 dae 的 是 存储 原始 XML 文件 的 ,文件 存储 为 zae 的 存储 压缩 的 XML 文件 的 。 为 
了 节省 空间 ,OpenRAVE 中 的 大 多 数 机 器 人 都 存储 为 zae。 

以 下 是 属性 可 以 传递 给 环境 载 人 和 读 出 的 方法 : 


skipgeonetry = "true" /"false" 

是 否 跳 过 几何 形状 。 

scalegeometry = "10 10 10" 

缩放 所 有 物品 的 所 有 几何 尺寸 。 
PREFIX = "newname " 

HINA EIH f Be KI AERA 
OpenRAVEscheme = "X1 X2" 


用 于 $ OPENRAVE DATA 路 径 的 外 部 引用 的 方案 使 用 x1. /或 x2: /指定 。 如 果 有 
管理 者 权限 ,请 使 用 x1: //authority。 这 些 方案 都 是 OpenRA VE 数据 库 的 别名 。 


uripassword = "URI password" 


为 要 在 加 密 存 档 时 对 添加 条 目 使 用 的 URI/ 密 钥 键 。 
以 下 是 属性 可 以 传递 给 环境 的 保存 和 写 入 方法 : 


externalref ="bodynamel bodyname2" 


如 果 写 人 collada ,请 指定 应 通过 外 部 引用 导出 的 名 称 。 如 果 为 *, 则 使 用 外 部 引用 导 
出 身体 部 位 。 因 为 用 户 可 能 对 机 器 人 参数 进行 了 本 地 修改 ,所 以 导出 的 内 容 取决 于 


forcewirte。 
ignoreexternaluri = "URI' 


一 组 URI 到 文档 ,这 些 文档 永远 不 会 被 正在 保存 的 当前 文档 外 部 引用 。 用 于 标记 临 
时 URI。 


Skipwrite = "optionl option2" 


跳 过 写 这 些 属性 。 支 持 的 选项 有 : * geometry -任何 < geometry > 对 象 * 可 读 。 
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- From.Interface.GetReadableInterfaces * sensor * manipulator * physics * visual 一 
«node» hierarchy * link collision state 


跳 过 写 和 链接 的 碰撞 状态 。 
forcewrite = "optionl option2" 


如 果 使 用 外 部 引用 , 则 强制 写 人 这 些 属性 。 这 些 属 性 可 以 在 运行 时 由 用 户 设置 ,并 且 是 
更 具体 的 应 用 程序 而 不 是 限定 于 机 器 人 。 如 果 为 * ,然后 强制 写 所 有 支持 的 选项 。 默 认 情 
况 下 ,这 些 值 将 被 假定 为 包含 在 外 部 参考 中 。 选 项 是 : 


* manipulator * sensor x* jointlimit - position, velocity, accel * jointweight - weights, 
resolution * readable 


通过 可 读 接口 的 参数 * link collision state 来 写 链接 冲突 状态 。 
OpenRAVEschenme = " customscheme" 


写 外 部 引用 的 方案 。 写 程序 将 尝试 将 本 地 系统 URI (file: /) 转 换 为 相对 于 $ 
OPENRAVE_DATA 路 径 的 相对 路 径 ,并 使 用 customscheme 作为 方案 。 


unit ="1.0" 

在 一 个 距离 单元 中 有 多 少 真实 世界 中 的 米 。 例 如 ,unit =“0. 001” 表 示 毫 米 。 
reusesimilar = "true" /" false" 

如 果 为 true, 则 尝试 重用 类 似 的 网 格 和 结构 以 减 小 大 小 。 

password = "????" 

任何 属性 都 可 以 通过 


collada - dom DAE :: getIOPlugin :: setOption 


来 设置 。 
因为 COLLADA 可 能 有 点 难以 手动 编辑 .OpenRAVE 还 定义 了 自己 的 格式 ,以 帮助 用 


户 快速 将 机 器 人 带 入 环境 。 可 以 使 用 以 下 命令 将 这 些 自 定义 robot 转换 为 COLLADA: 


OpenRAVE - save myrobot.zae myrobot. xml 
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5.2.5 插件 与 接口 说 明 


1. 编写 插件 与 接口 

每 个 插件 需要 导出 几 个 函数 ,如 插件 导出 函数 中 定义 ,以 通知 OpenRAVE 它 有 什么 接 
口 。 当 插件 首次 加 载 时 , 它 由 环境 验证 ,并 且 它 的 OpenRAVEGetPluginAttributes 函数 将 
被 调用 ,以 便 OpenRAVE 核心 可 以 注册 其 提供 的 接口 的 名 称 。 插 件 本 身 可 以 通过 环境 的 
接口 查询 功能 查询 其 他 插件 提供 的 功能 。 

1) 制作 一 个 简单 的 接口 

下 面 的 示例 plugincpp. cpp 是 创建 一 个 名 为 MyModule 的 OpenRAVE:: ModuleBase 
接口 ,并 提供 两 个 命令 : numbodies 和 load, 4 PE 28 A A 0g 4$ — 4- H include 必须 是 
OpenRAVE/OpenRAVE.h。 然 后 对 于 主要 的 C++ 文件 ,包括 OpenRAVE/ plugin. h ft JL 
个 帮助 函数 。 


# include < OpenRAVE/OpenRAVE. h > 
# include < OpenRAVE/plugin. h> 

# include < boost/bind. hpp> 
using namespace std; 

using namespace OpenRAVE; 
namespacecppexamples { 
classMyModule :publicModuleBase 
{ 


现在 注册 模块 的 两 个 命令 。boost::bind 是 指定 成 员 函 数 作为 回调 所 必需 的 : 


MyModule(EnvironmentBasePtrpenv) : ModuleBase(penv) 

{ 

. description = "A very simple plugin."; 

RegisterCommand( " numbodies", boost:: bind (&MyModule:: NumBodies, this, _ 1, _ 2)," returns 
bodies"); 

RegisterCommand(" load", boost: :bind(&MyModule::Load, this, 1, 2), "loads a given file"); 

) 


提供 成 员 函 数 的 实现 : 


boolNumBodies(ostream&sout, istream&sinput) 
{ 
vector <KinBodyPtr > vbodies; 
GetEnv() — > GetBodies(vbodies) ; 
sout << vbodies.size(); //publish the results 
returntrue; 


y 
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bool Load(ostream&sout, istream&sinput) 
Jn 
string filename; 
sinput >> filename; 
boolbSuccess = GetEnv() -> Load(filename. c_str()); //load the file 
returnbSuccess; 
) 
Nh 


建议 插件 作者 在 其 主要 的 C++ 文件 中 包含 OpenRA VE/plugin. h, 这 将 提供 导出 函数 
的 实现 ,并 要 求 用 户 提供 一 组 新 的 函数 Create Interface Validated 和 Get Plugin Attributes 
Validated, 


提供 MyModule 会 看 起 来 像 : 


InterfaceBasePtrCreateInterfaceValidated(InterfaceType type, conststd: :string&interfacename, 
Std::istream&sinput, EnvironmentBasePtrpenv) 

{ 

if( type == PT Module&&interfacename == "mymodule" ) { 
returnInterfaceBasePtr(newcppexamples: : MyModule( penv) ) ; 

) 


为 了 告诉 OpenRAVE 我 们 提供 了 什么 .必须 定义 : 


voidGetPluginAttributesValidated(PLUGININFO& info) 

il 

info. interfacenames[PT Module].push back("MyModule"); 
) 


2) 制作 插件 
OpenRAVE 的 主 构建 系统 是 cmake.FindOpenRAVE. cmake 可 用 于 查找 OpenRAVE 
安装 。 使 用 FindOpenRAVE. cmake 编译 插件 的 CMakeLists. txt 文件 示例 如 下 : 


cmake minimum required (VERSION 2.6) 
project (plugincpp) 
find package(OpenRAVE REQUIRED) 
include directories( $ (OpenRAVE INCLUDE DIRS]) 
link directories( $ (OpenRAVE LIBRARY DIRS]) 
add library(plugincpp SHARED plugincpp. cpp) 
set target properties(plugincpp PROPERTIES COMPILE FLAGS " $ (OpenRAVE CXX FLAGS)" LINK 
FLAGS " $ (OpenRAVE LINK FLAGS]") 
target link libraries(plugincpp $ (OpenRAVE LIBRARIES])) 


如 果 不 使 用 CMake, 那 么 开发 文件 的 组 织 方式 如 下 : Linux 用 户 根据 OpenRAVE 的 安装 位 
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置 ,应 在 $ OPENRAVE_INSTALL /bin 目录 中 创建 OpenRAVE-config。 可 以 调用 OpenRAVE- 
config-cflags 来 获取 正确 的 路 径 和 标志 ,以 包含 在 gcc 中 以 链接 到 libOpenRAVE. so 中 。 

30 使 用 插件 

有 几 种 方法 来 加 载 生成 的 插件 。 最 简单 的 方法 是 将 其 安装 目录 添加 到 OPENRAVE_ 
PLUGINS, OpenRAVE 将 在 启动 时 自动 加 载 它 。 用 户 可 以 使 用 以 下 方法 确认 : 


OpenRAVE -- listplugins 


更 明确 的 方法 是 使 用 以 下 任何 一 种 方法 从 命令 行 加 载 它 : 


OpenRAVE —- loadplugin $ SOMEPATH/libplugincpp 
OpenRAVE —- loadplugin $ SOMEPATH/libplugincpp. so 
OpenRAVE —- loadplugin ./libplugincpp. so 


其 中 ,$ SOMEPATH 是 共享 对 象 的 绝对 /相对 路 径 。 
另 一 种 方法 是 从 C++/Python/API 加 载 它 : 
使 用 C++ 语言 时 


RaveLoadPlugin(env, "plugincpp"); 


使 用 Python 语言 时 : 


RaveLoadPlugin( 'plugincpp') 


使 用 Octave 语言 时 : 
orEnvLoadPlugin( 'plugincpp'); 


一 旦 插件 被 加 载 ,我们 可 以 创建 接口 并 调用 其 命令 来 加 载 环 境 并 返回 主体 数量 : 
使 用 C++ 语言 时 : 


ModuleBasePtrprob = RaveCreateModule(env, "MyModule"); 
env — > AddModule(prob, "") ; 

stringstreamsinput, sout; 

//input the load command 

sinput ««" load data/labl. env. xnl"; 

if( ! prob- > SendConmand( sout, sinput) ) { 

RAVELOG WARN("command failed!Wn"); 

J 


else { 
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sinput.str(""); //have to reset the stream from the previous command 
sinput -««"numbodies"; //input the numbodies command 

prob - > SendCommand( sout, sinput); 

intnumbodies; 

sout >> numbodies; 

RAVELOG INFO("number of bodies are: % d\n", numbodies); 

} 


使 用 Python 语言 时 : 


prob = RaveCreateModule(env, 'MyModule') 

env. AddModule(prob, args = '') 

cmdout = prob. SendCommand( 'load data/labl.env.xml') 
ifcmdout is None: 

raveLogWarn( 'command failed! ') 

else: 

cmdout = prob. SendCommand( 'numbodies') 

print 'number of bodies are: ',cmdout 


使 用 Octave 语言 时 : 


prob = orEnvCreateProblem( 'MyModule'); 
orProblemSendCommand( 'load data/labl.env.xml',prob); 
numbodies = orProblemSendCommand( 'numbodies', prob) ; 
disp(['number of bodies are: ' num2str(numbodies)]) 


4) 记录 接口 

所 有 接口 文档 的 格式 是 广泛 采用 的 标准 reStructuredText。 接 口 的 描述 和 关于 它 的 使 
用 的 所 有 信息 应 该 由 两 个 地 方 提供 : 

(1) OpenRAVE :: InterfaceBase :: GetDescription() 

返回 接口 描述 的 完整 文档 。 如 果 打 开 新 的 部 分 ,不 要 使 用 ”-”。 

reStructuredText 格式 的 接口 文档 。“ 多 线程 安全 ”定义 在 文件 interface. h( 也 就 是 上 
面 的 代码 ) 的 第 84 行 。 

代码 段 链 接 网 址 为 : http://www. OpenRAVE. org/docs/latest_ stable/coreapihtml/ 
interface_8h_source. html, 

(2) OpenRAVE :: InterfaceBase :: RegisterCommand() 

在 每 个 命令 注册 的 帮助 字符 串 。 如 果 打 开 新 的 部 分 ,不 要 使 用 *-”,“ 二 "和 “~”。 


voidOpenRAVE: : InterfaceBase: :RegisterCommand(conststd: :&cmdname 
InterfaceBace: :InterfaceConmandFnfncnd 
conststd::string & strhelp 
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其 中 , cmdname 为 命令 名 称 , 转换 为 小 写 ; fncmd 函数 为 命令 执行 ; strhelp 为 
reStructuredText 中 的 帮助 字符 串 。 在 文件 interface. cpp 的 第 121 行 定义 。 

代码 段 链接 网 址 为 : http://www. OpenRAVE. org/docs/latest_stable/coreapihtml/ 
interface_8cpp_source. html, 

5) 加 载 插件 

已 经 设置 了 许多 机 制 以 防止 由 核 加 载 到 不 匹配 的 或 者 旧 的 插件 。 使 用 旧 的 插件 的 界面 
可 能 导致 意外 的 崩溃 ,这 很 难 调试 。 可 以 通过 C++ 的 lexer 运行 每 个 接口 ,然后 创建 一 个 
128 位 的 唯一 的 md5hash, 自 动 地 得 到 接口 函数 和 成 员 的 唯一 hash。 为 了 保护 使 用 不 同 版 
本 编译 的 插件 ,OpenRAVE 使 用 cpp-gen-md5 从 每 个 接口 类 定义 创建 一 个 md5hash, 并 将 
它们 存储 在 OpenRAVE /interfacehashes. h 中 。 可 以 使 用 OpenRAVE :: RaveGetInterfaceHash 
检索 接口 hash。 对 于 要 成 功 加 载 的 接口 ,插件 必须 检查 核心 使 用 的 hash 是 否 与 使 用 插件 
编译 的 hash 匹配 。 这 些 类 型 的 检查 确保 永远 不 会 加 载 过 时 的 插件 ; 辅助 函数 在 
OpenRA VE/plugin. h 中 提供 ,作者 应 该 使 用 所 有 插件 。 

2. 基本 接口 的 概念 

新 接口 由 插件 提供 ,并 动态 加 载 到 OpenRAVE 中 。 所 有 接口 派生 自 OpenRAVE :: 
InterfaceBase 类 ,并 包含 基本 信息 ,如 类 型 .拥有 环境 .设置 用 户 数据 ,复制 以 及 允许 发 送 自 
定义 字符 串 命令 。 每 个 实例 化 接口 仅 属于 一 个 环境 。 可 以 使 用 OpenRAVE :: InterfaceBase :: 
Clone 复制 接口 。 每 个 接口 都 可 以 有 自己 的 自 定 义 命令 。 发 送 帮 助 将 返回 接口 支持 的 所 有 
命令 的 列表 (认为 它 是 向 接口 发 送 命令 的 命令 行 方式 )。GetDescription() 返 回 一 个 简要 说 
明 功 能 ,作者 和 插件 许可 证 的 字符 串 。 能 够 注册 自 定义 xml 阅读 器 接口 。 

3. OpenRAVE 目前 主要 接口 

OpenRAVE 标识 可 以 由 插件 实现 的 特定 接口 类 别 。 目 前 主要 的 接口 类 型 有 : 

1) 规划 器 

规划 是 机 器 人 为 了 在 保持 某 些 约束 (例如 保持 动态 平衡 或 避免 与 障碍 物 的 碰撞 ) 的 同 
时 ,从 其 初始 状态 到 目标 状态 必须 遵循 的 轨迹 或 策略 。 规 划 器 从 初始 条 件 产生 计划 。 

在 OpenRAVE 中 具体 的 规划 器 使 用 参见 网 址 : http://www. OpenRAVE. org/docs/ 
latest stable/coreapihtml/arch planner. html. 

2) 控制 器 

每 个 机 器 人 都 要 连接 到 控制 器 中 ,用 于 在 其 所 在 环境 (模拟 或 实际 ) 中 移动 它 。 控 制 器 
提供 获取 或 设置 轨迹 ,并 查询 机 器 人 当前 状态 的 功能 。 

在 OpenRAVE 中 具体 的 控制 器 使 用 参见 网 址 : http://www. OpenRAVE. org/docs/ 


latest stable/coreapihtml/arch controller. html, 


3) 传感器 
传感器 (如 测 距 仪 或 相机 ?收集 有 关 环境 中 的 信息 ,并 以 标准 格式 返回 。 传 感 器 可 以 连 
接 到 机 器 人 的 任何 部 分 。 


在 OpenRAVE 中 具体 的 传感器 使 用 参见 网 址 : http://www. OpenRAVE. org/docs/ 
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latest stable/coreapihtml/arch sensor. html. 

4) 传感器 系统 

可 以 根据 某 些 外 部 输入 设备 (如 运动 捕 提 系统、 视觉 相机 或 激光 测 距 数据 ), 从 而 任意 更 
新 对 象 姿态 估计 数据 的 系统 。 

在 OpenRAVE 中 具体 的 传感器 系统 使 用 参见 网 址 : http://www. OpenRAVE. org/ 
docs/latest_stable/coreapihtml/arch_sensorsystem. html, 

5) 问题 实例 

fi [i] ESI [BI JS LT EA. OpenRAVE 中 的 小 程序 。 创 建 后 ,通过 SendMessage 函数 
向 主 仿真 循环 和 OpenRAVE 网 络 服务 器 注册 问题 实例 。 问 题 实例 可 以 提供 操作 或 导航 的 
特殊 功能 ,并 且 可 以 轻松 扩展 OpenRAVE 的 网 络 功能 。 

6) 机 器 人 

OpenRAVE 支持 具有 独特 功能 的 机 器 人 的 各 种 不 同 的 运动 结构 。 例 如 ,用 于 类 人 机 器 
人 的 接口 明显 不 同 于 轮 式 移动 机 器 人 的 接口 。 提 供 各 种 类 型 的 机 器 人 的 实现 使 客户 能 够 更 
好 地 利用 其 结构 。 

在 OpenRAVE 中 具体 的 机 器 人 使 用 参见 网 址 : http://www. OpenRAVE. org/docs/ 
latest stable/coreapihtml/arch robot. html. 

7) 反 向 运动 学 求解 器 

可 以 指定 IKCInverse Kinematics Solvers) 求 解 器 ,并 返回 封闭 解 或 数值 解 , 可 用 作 操 作 
规划 器 的 输入 。 每 个 IK 解 算 器 可 以 附加 到 机 器 人 的 链接 的 子 集 。 

在 OpenRAVE 中 具体 的 OpenRAVE :: IkSolverBase 类 参见 网 址 : http://www. 
OpenRAVE. org/docs/latest_stable/ coreapihtml/classOpenRAVE _1_1IkSolverBase. html, 

8) 物理 引擎 

OpenRAVE 提供 了 通过 插件 使 用 任何 自 定义 仿真 系统 库 的 能 力 , 而 无 须 任何 其 他 插 
件 , 也 无 须 了 解 库 的 细节 或 如 何 链 接 。 

在 OpenRAVE 中 具体 的 物理 引擎 使 用 参见 网 址 : http://www. OpenRAVE. org/ 
docs/latest stable/coreapihtml/arch physicsengine. html. 

当前 OpenRAVE 模型 会 加 载 同 一 主要 进程 中 的 所 有 插件 。 在 兼容 性 方面 ,这 比 将 每 
个 插件 作为 独立 过 程 与 另 一 层 通信 的 灵活 性 要 小 。 然 而 ,此 设计 决策 的 原因 是 ,可 能 需要 以 
高 频率 (例如 每 秒 秒 数 千 次 ) 调 用 一 些 消息 和 函数 调用 ,例如 碰撞 查询 。 例 如 ,运动 计划 器 可 
以 在 搜索 期 间 针 对 每 个 候选 机 器 人 对 CheckCollision 函数 进行 调用 。 虽 然 可 以 使 用 网 络 协 
议 来 执行 这 些 查 询 , 但 是 跳 转 到 存储 器 地 址 比 通 过 共享 存储 器 使 用 TCP/IP 要 高 效 得 多 。 
类 似 地 ,在 紧密 循环 中 执行 的 复杂 运动 学 或 物理 仿真 的 查询 需要 从 调用 者 中 抽象 实现 ,并 且 
查询 本 身 又 要 尽 可 能 快 。 鉴 于 这 些 考 虑 .在 主要 进程 中 加 载 所 有 插件 的 设计 决策 就 不 像 最 
初 看 起 来 那样 严格 。 计 划 程 序 和 其 他 功能 仍然 能 够 在 多 个 线程 上 运行 ,并 且 插 件 可 以 根据 
需要 通过 网 络 或 共享 内 存 连接 到 其 他 系统 。 此 外 .可 以 存在 多 个 主要 进程 ,每 个 实例 加 载 其 
自己 的 一 组 插件 并 运行 其 自己 的 一 组 计划 器 。 
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5.2.6 ”网络 协议 和 脚本 


简要 总 结 了 OpenRAVE 网 络 服务 器 ,可 用 的 命令 提供 了 以 下 服务 : 
。 与 机 器 人 控制 器 通信 。 
。 读 取 场 景 对 象 ,机 器 人 关节 值 ,链接 转换 和 链接 几何 的 状态 估计 。 
设置 对 象 姿势 和 机 器 人 关节 角度 值 。 
执行 对 象 到 对 象 或 物体 到 对 象 的 碰撞 查询 。 
。 创建 或 销毁 任何 机 器 人 、 对 象 . 环 境 或 问题 实例 。 
。 向 问题 实例 发 送 命令 并 获取 结果 。 
在 OpenRAVE 3D 环境 可 视 化 GUI 中 绘制 点 云 ` 线 和 其 他 基 元 。 
加 载 和 重新 加 载 插件 。 

。 设置 调试 模式 ,调整 规划 ,仿真 和 GUI 参数 。 

在 当前 实现 中 ,网 络 命令 通过 TCP/IP 发 送 并 且 是 基于 文本 的 。 基 于 文本 的 命令 允许 
对 数据 进行 简单 的 解释 ,并 使 支持 的 脚本 语言 变 得 直接 ,并 且 协 议 不 限于 网 络 和 TCP/IP 连 
接 。 在 将 来 ,我 们 计划 通过 类 似 于 ROS [3] 的 XML-RPC 实现 仲裁 层 ,可 以 决定 在 脚本 、 
GUI 和 Core 层 之 间 传 输 数 据 的 最 佳 格式 和 方法 。 例 如 ,在 本 地 运行 到 主要 进程 的 脚本 时 
应 该 自动 利用 共享 内 在 两 个 进程 之 间 进 行 通信 。OpenRAVE 目前 支持 通过 网 络 套 接 字 进 
行 通信 的 Octave AI MATLAB 脚本 环境 。 与 OpenRA VE 功能 的 交互 是 无 颖 的 。 用 户 只 需 
设置 运行 OpenRAVE 的 主机 的 IP 地 址 ,所 有 其 他 详细 信息 将 自动 被 处 理 。 


5.3 OpenRAVE 的 基础 


5.3.1 开始 使 用 OpenRAVE 


OpenRAVEpy 软件 包 人 允许 Python 无 颖 使 用 C++API。 绑 定 是 使 用 Boost. Python JÆ 
开发 的 ,而 且 因 为 OpenRAVEpy 是 直接 链接 到 OpenRAVE 而 不 是 通过 网 络 连接 , 它 允 许 
更 自然 地 进行 设置 ,并 有 着 更 短 的 执行 时 间 。 事 实 上 ,大 多 数 python 绑 定 与 精确 的 C++ 头 
文件 完全 匹配 。 

这 里 的 主要 组 成 部 分 是 : 

* OpenRAVEpy int 

* OpenRAVEpy ext 


提供 C++ 内 部 绑 定 ,是 使 用 Boost Python 生成 的 。 
OpenRAVEpy. ext 提供 有 用 的 函数 /类 以 供 其 余 类 使 用 。 


有 3 个 主要 组 件 : 
* 数据 库 软 件 包 一 一 数据 库 发 电机 。 
。 示例 包 一 一 可 运行 的 例子 。 


。 接口 封装 一 一 通过 OpenRAVE 插件 提供 的 绑 定 接口 。 
在 Windows 上 ,可 以 在 C: NN Program FilesNNVOpenRAVENN share\ \OpenRA VE 到 
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OpenRAVEpy。 对 于 基于 Unix 的 系统 ,后 续 的 命令 可 用 于 检索 路 径 : 


OpenRAVE — config —- python— dir 


当 直 接 导 入 OpenRAVEpy 时 ,此 路 径 需要 被 加 入 到 PYTHONPATH 的 环境 变量 中 。 
对 于 基于 Unix 的 系统 , 它 看 起 来 像 : 


exportPYTHONPATH = $ PYTHONPATH: 'OpenRAVE- config —- python- dir' 


所 有 的 例子 都 存储 在 OpenRAVEpy/examples。 例 如 ,最 简单 的 规划 示例 可 以 在 
OpenRAVEpy/examples/hanoi. py 找到 ,并 执行 ,通过 命令 : 


OpenRAVE.py —-— example hanoi 
每 个 函数 和 类 的 文档 字符 串 会 自动 从 C++ 编译 文件 。 只 需 在 Python 解释 器 键入 : 


helpenv. CloneSelf # env is an instance of Environment() 
helpKinBoby.GetChainz KinBody is a class 
helpRobot. Manipulator. FindIKSolution# Robot.Manipulator is a sub- class 


1. 异常 情况 
OpenRAVE C++ 异常 以 OpenRAVE_exception 类 的 形式 自动 转换 为 python 中 的 
OpenRAVE_exception 类 。OpenRAVE 异常 可 以 通过 以 下 方式 捕获 : 


try: 
env= Environment() 
env. Load( 'robots/barrettwam. robot. xml') 
env. GetRobots() [0]. SetDOFValues([]) 
exceptOpenRAVE exception, e: 
print e 


2. 锁定 与 线程 安全 机 制 

当 执行 繁重 的 操作 时 ,应 始终 锁定 环境 以 防止 其 他 用 户 更 改 它 。 所 有 环境 都 是 多 线程 
安全 的 ,但 是 如 果 文 档 没有 说 多 线程 安全 ,那么 任何 其 他 方法 对 kinbodies、 机 器 人 、 控 制 器 、 
规划 器 等 而 言 都 不 是 线程 安全 的 ! 千 万 不 要 尝试 没有 锁定 环境 的 方法 ! 

锁定 是 使 用 Environment. Lock (dolock) 完 成 的 。 范 围 锁定 可 以 使 用 try/finally 块 或 
在 python 中 通过 下 面 语句 实现 : 


env= Environment() 
# initialization code 
withenv: 

# environment is now locked 
env. CheckCollision(...) 
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类 似 地 ,在 结构 和 机 器 人 上 使 用 类 似 的 声明 来 锁定 环境 并 保持 它们 的 状态 : 


with robot: 
robot. SetTransform( newtrans) 
robot. SetActiveDOFs(...) 
# do work 


# robot now has its previous state restored 


对 于 那些 想 减 少 环境 锁 数量 的 人 ,可 以 使 用 新 的 KinBodyStateSaver 和 
RobotStateSaver 2 ; 


withenv: 
# enviroment locked at this point 
withKinBodyStateSaver( body): 
* body state now preserved 
withRobotStateSaver( robot): 

# robot state now preserved 


3. 初始 化 

Ravelnitialize( ) 初 始 化 OpenRAVE 运行 ,并 提供 了 许多 配置 选项 。 选 项 包括 在 启动 时 
加 载 的 插件 。 如 果 在 创建 Environment 时 未 运行 初始 化 时 ,系统 则 会 自动 调用 
RaveInitialize( ) 。 


以 下 示例 显示 如 何 启动 运行 并 仅 加 载 一 个 插件 : 


try: 
Ravelnitialize(load all plugins = False) 
success = RaveLoadPlugin( 'libbasemanipulation') 
* do work 
finally: 

RaveDestroy() # destroy the runtime 


4. 销毁 
由 于 与 内 部 OpenRAVE 资源 的 循环 依赖 关系 ,环境 实例 必须 使 用 Environment. 
Destroy 进行 破坏 。 为 了 保证 它 总 是 被 调用 ,建议 用 户 使 用 try/finally: 


try: 
env= Environment() 
# do work 
finally: 
env. Destroy() 


此 外 , 当 用 户 关闭 程序 时 ,必须 使 用 RaveDestroy() 显 式 地 销毁 管理 插件 资源 和 环境 。 
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它 会 破坏 所 有 环境 并 印 载 所 有 插件 : 


try: 
envl = Environment() 
env2 = Environment() 
RaveLoadPlugin( myplugin') 
# do work 
finally: 
RaveDestroy( ) # destroys all environments and loaded plugins 


5. 加 载 不 同 版 本 
如 果 安 装 了 多 个 OpenRAVE 版 本 , 则 可 以 在 导入 任何 内 容 之 前 通过 将 _ 
OpenRAVEpy version _ 变量 设置 为 所 需 的 版 本 来 选择 OpenRAVEpy 的 版 本 。 例 如 : 


. builtins .  OpenRAVEpy version — - '0.4" 

importOpenRAVEpy 

6. 记录 

可 以 使 用 DebugLevel 逐个 设置 内 部 OpenRAVE 的 日 志 记 录 级 别 : 


RaveSetDebugLevel(DebugLevel. Verbose) 


很 多 OpenRA VE Python 绑 定 直接 使 用 python 日 志 模块 。 为 了 使 用 正确 的 输出 句 式 ， 
初始 化 它 , 并 使 其 与 内 部 OpenRAVE 日 志 记录 级 别 同 步 ,请 使 用 以 下 命令 : 


fromOpenRAVEpy.miscimportInitOpenRAVELogging 
InitOpenRAVELogging() 


5.3.2 OpenRAVE 的 命令 行 工 具 


1. OpenRAVE. py 

OpenRAVE. py 脚本 试图 使 OpenRAVE 的 命令 行 参数 更 容易 使 用 。 它 是 原始 
OpenRA VE 程序 提供 的 函数 的 超 集 , 它 除 了 支持 许多 其 他 有 趣 的 功能 , 并 为 所 有 
OpenRA VEpy 函数 提供 一 个 窗口 , 它 还 可 以 自动 添加 OpenRAVEpy 到 PYTHONPATH， 
使 其 更 简单 。 这 里 有 一 些 它 支持 的 功能 。 

在 加 载 指 定 的 特定 文件 后 使 用 -i 选项 打开 文件 现在 会 放 入 ipython 解释 器 。 例 如 : 


OpenRAVE.py — i data/labl.env.xml 


输出 为 : 
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[OpenRAVEpy int.cpp:2679] viewer qtcoin successfully attached 
OpenRAVE Dropping into IPython 
In [1]: 


场景 中 的 第 一 个 机 器 人 自动 加 载 到 "robot" 变 量 中 ,因此 可 以 立即 用 于 脚本 操作 : 


In [1]: robot.GetJoints() 
Out[1]: 
[< env. GetKinBody('BarrettWAM'). GetJoint('Shoulder Yaw')», 
< env. GetKinBody( 'BarrettWAM').GetJoint('Shoulder Pitch')», ...] 


可 以 启动 数据 库 生 成 过 程 : 


OpenRAVE.py —- database inversekinematics —- robot = robots/pal0. robot. xml 


可 以 执行 一 个 例子 : 

OpenRAVE.py -- example graspplanning 
可 以 查询 所 有 可 执行 数据 库 : 
OpenRAVE.py — listdatabases 


输出 为 : 


No module named DatabaseGenerator 
No module named OpenRAVEGlobalArguments 
convexdecomposition 

No module named getenv 

grasping 

No module named h5py 
inversekinematics 
inversereachability 
kinematicreachability 
linkstatistics 

No module named log 

No module named logging 

No module named makedirs 

No module named metaclass 

No module named OpenRAVEpy int 

No module named os 

No module named pickle 

No module named time 

No module named version info 
visibilitymodel 
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No module named with statement 
可 以 设置 自 定义 碰撞 ,物理 状态 和 查看 器 : 
OpenRAVE.py —— collision = pqp —- viewer = qtcoin —- physics = ode data/lab1. env. xml 
可 以 设置 调试 方式 : 
OpenRAVE.py —- level = verbose data/labl. env. xml 
可 以 执行 任意 python 代码 : 


OpenRAVE.py — p "print 'robot manipulators: ', robot. GetManipulators( )" robots/pr2 - beta — 
sim.robot.xml 


可 以 执行 任意 python 代码 ,并 进入 ipython 解释 器 : 


OpenRAVE.py — p "manip = robot. GetActiveManipulator()" - i robots/pr2 - beta - sim. robot. xml 


可 以 执行 任意 python 代码 并 退出 : 


OpenRAVE.py — p "print('links: ' + str( robot. GetLinks())); sys.exit(0)" robots/pr2 - beta — 
sim.robot.xml 


鉴于 环境 xml 文件 现在 可 以 包含 任何 接口 的 标签 ,可 以 在 XML 中 设置 所 有 使 用 的 接 
H «fii JH OpenRA VE. py -i 打开 它 , 并 立即 开始 对 状态 进行 内 部 设 定 。 
输入 以 下 命令 : 


Usage: OpenRAVE. py [options] [loadable OpenRAVE xml/robot files...] 


OpenRAVE 0.9.0 


Options: 
—- version show program's version number and exit 
一 hb，-- help Show this help message and exit 
—- database If specified, the next arguments will be used to call 


a database generator from the OpenRAVEpy. databases module. The first argument is used to find 
the database module. For example: OpenRAVEO.9.py 
—- database grasping 
—- robot = robots/pr2 - beta — 
sim.robot. xml 
—- example If specified, the next arguments will be used to call 
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an example from the OpenRAVEpy. examples module. The first argument is used to find the example 
moduel. For example: OpenRAVEO.9.py —- example graspplanning 
—- scene = data/labl. env. xml 
— i, —- ipython if true will drop into the ipython interpreter rather 


Execute a python command after all loading is done and 
before the drop to interpreter check. The variables available to use are: "env", "robots", 
"robot". It is possible to quit the program after the command is executed by adding a "sys. exit 
(0)" at the end of the conmand. 
=- listinterfaces = LISTINTERFACES 
List the provided interfaces of a particular type from 
all plugins. Possible values are: planner, robot, sensorsystem, controller, module, iksolver, 
kinbody, physicsengine, sensor, collisionchecker, trajectory, viewer, spacesampler. 
=- listplugins List all plugins and the interfaces they provide. 
listdatabasesLists the available core database generators 
=- listexamplesLists the available examples. 


OpenRAVE Environment Options: 
—- loadplugin = IOADPLUGINS 
List all plugins and the interfaces they provide. 
—- collision- COLLISION 
Default collision checker to use 
—- physics = PHYSICS physics engine to use (default = none) 
—- viewer = VIEWER viewer to use (default = qtcoin) 
—- server = SERVER server to use (default = None). 
—- serverport = SERVERPORT 
port to load server on (default = 4765). 
—- module - MODULES module to load, can specify multiple modules. Two 
arguments are required: "name" "args". 
一 1 LEVEL, -- level- LEVEL, -- log level- LEVEL 
Debug level, one of 
(fatal, error, warn, info, debug, verbose, verifyplans) 


2. OpenRAVE-robot. py 
能 查询 有 关 OpenRAVE 可 装载 的 机 器 人 的 信息 。 人 允许 尽 可 能 快 地 查询 机 器 人 连 杆 、 
关节 、 操 纵 器 ,传感器 的 简单 信息 。 例 如 ,获取 所 有 操纵 器 的 信息 ,代码 如 下 : 


OpenRAVE — robot. py robots/pr2 - beta- static. zae —— info manipulators 


或 者 可 以 只 获取 操纵 器 名 称 的 列表 : 
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OpenRAVE — robot. py robots/pr2 - beta- static. zae —— list manipulators 


每 个 机 器 人 可 以 根据 被 查询 的 信息 保存 几 种 不 同类 型 的 hash fH. hash 值 使 用 -hash 
选项 检索 ; 


OpenRAVE — robot. py data/mugl.kinbody.xml —— hash body 
OpenRAVE - robot. py robots/barrettsegway.robot.xml —— hash robot 
OpenRAVE — robot. py robots/barrettsegway.robot.xml —- manipname = arm —- hash kinematics 


输入 以 下 命令 行 : 
Usage: OpenRAVE - robot. py OpenRAVE - filename [options] 


Queries information about OpenRAVE - loadable robots 


Options: 

=h, —- help show this help message and exit 

—- list = DOLIST Lists the manipulators/sensors/links/joints names of 
the robot. 

—- info = DOINFO Prints detailed information on 
manipulators/sensors/links/joints information of a 
robot. 

—- hash = DOHASH If set, will output hashes of the loaded body 


depending if manipname or sensorname are set. Can be 
one of (body, kinematics, robot) 

—- manipname = MANIPNAME 
if manipulator name is specified will return the 
manipulator hash of the robot 

—- sensorname = SENSORNAME 
if manipulator name is specified will return the 
sensor hash of the robot 


3. OpenRAVE-createplugin. py 


用 于 设置 项 目 目录 和 用 于 创建 OpenRA VE 插件 和 可 执行 文件 的 初始 文件 。 
下 面 这 个 命令 行将 创建 一 个 插件 ,提供 一 个 MyNewModuleModuleBase: 


OpenRAVE - createplugin.pymyplugin -- module MyNewModule 
输入 以 下 命令 行 : 
Usage: OpenRAVE - createplugin. py pluginname [options] 


Sets up a project directory and initial files for creating OpenRAVE plugins 
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and executables. 


Options: 

=b, == help show this help message and exit 

—- usecore If set, will create an executable that links to the 
core instead of creating a plugin. 

—- planner - PLANNER create a planner interface 

—- robot - ROBOT create a robot interface 


—- sensorsystem - SENSORSYSTEM 

create a sensorsystem interface 
—- controller = CONTROLLER 

create a controller interface 
—- module - MODULE create a module interface 
—- iksolver- IKSOLVER create a iksolver interface 
—- kinbody - KINBODY create a kinbody interface 
—- physicsengine - PHYSICSENGINE 

create a physicsengine interface 
—- sensor - SENSOR create a sensor interface 
—- collisionchecker - COLLISIONCHECKER 

create a collisionchecker interface 
—- trajectory - TRAJECTORY 

create a trajectory interface 
—- viewer - VIEWER create a viewer interface 
—- spacesampler - SPACESAMPLER 

create a spacesampler interface 


4. OpenRAVE 


能 用 C++ 编写 的 可 以 启动 OpenRA VE. 环境 和 加 载 模块 的 简单 可 执行 文件 。 它 提供 简 
单 的 参数 配置 ,便于 测试 。 可 以 将 机 器 人 保存 到 其 中 : 


[OpenRAVE. cpp:93] OpenRAVE Usage 


=- nogui Run without a GUI (does not initialize the graphics engine nor communicate 
with any window manager) 

—- hidegui Run with a hidden GUI, this allows 3D rendering and images to be captured 
—- listplugins List all plugins and the interfaces they provide 


—- loadplugin [path] load a plugin at the following path 

—- serverport [port] start up the server on a specific port (default is 4765) 

—- collision [name] Default collision checker to use 

—- viewer [name] Default viewer to use 

—- server [name] Default server to use 

—- physics [name] Default physics engine to use 

—d [debug - level] start up OpenRAVE with the specified debug level (higher numbers print 
more). 


Default level is 2 for release builds, and 4 for debug builds. 
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—wdims [width] [height] start up the GUI window with these dimensions 

— wpos x y set the position of the GUI window 

—- module [modulename] [args] Start OpenRAVE with a module. If args involves spaces, surround 
it with double quotes. args is optional. 

—- version Output the current OpenRAVE version 


-f [scene] Load aO0penRAVE environment file 


5. OpenRAVE-config 

用 于 查找 OpenRAVE 安装 目录 ,使 用 的 库 , 标 题 和 共享 文件 。 

输入 以 下 命令 : 

Usage:OpenRAVE - config [ -- prefix[ = DIR]] [ -- exec - prefix[ = DIR]] [ -- version] [-- 
cflags] [ -- libs] [ -- libs- core] [--1libs-only-1] [ -- libs - only- L] [ -- cflags - only 
- I] [ -- shared- libs] [ -- python- dir] [ -- octave - dir] [ ~- matlab- dir] [ -- share - dir] 
[-- usage | -- help] 


5.3.3 "5 OpenRAVE 文档 


1. 插件 

创建 OpenRAVE 插件 允许 其 他 人 通过 RaveCreateX. 方法 来 连接 接口 ,从 而 看 到 和 使 
用 用 户 的 工作 。 虽 然 强烈 建议 开始 使 用 Python/Octave, 但 最 终 也 是 用 户 应 该 创建 插件 以 
通过 它们 提供 相应 的 功能 。 

创建 插件 的 最 简单 的 方法 是 通过 OpenRAVE-createplugin. py 程序 。 

例如 ,以 下 命令 将 创建 一 个 提供 MyNewModule Module 的 插件 : 


OpenRAVE - createplugin.pymyplugin -- module MyNewModule 
这 将 创建 一 个 myplugin 目录 ,其 中 写 人 所 有 的 文件 。 下 面 是 为 了 编译 和 测试 它 : 
cdmyplugin 


make 
python testplugin. py 


默认 情况 下 ,新 的 插件 /可 执行 文件 存储 在 构建 文件 夹 中 。 创 建 以 下 文件 以 帮助 用 户 
开始 : 

CMakeLists. txt 一 一 用 于 创建 Makefiles 和 Visual Studio 解决 方案 。 

Makefile 对 于 Unix 用 户 , 键 人 “make” 来 构建 CMake 项 目 。 

myplugin. cpp 一 一 主要 的 C++ 文 件 。 

testplugin. py 一 一 将 加 载 OpenRAVE 中 的 插件 并 调用 其 SendCommand 功能 。 
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scenes/robots 保存 场景 和 机 器 人 文件 的 目录 。 
2. 编程 
也 可 以 通过 与 OpenRAVE-core 库 链接 来 在 程序 中 创建 和 使 用 OpenRAVE, 创建 示 


例 程 序 的 最 简单 的 方法 是 : 


OpenRAVE — createplugin.pymyprogram 一 一 USecore 


这 是 创建 一 个 OpenRA VE 环境 和 加 载 场景 的 简单 程序 。 创 建 环境 时 不 附加 任何 查 


5.3.4 环境 变量 


1. OPENRAVE_DATA 

搜索 路 径 /URL 用 于 加 载 robot/environment/model 文件 。 使 用 OpenRAVE 安装 的 
机 器 人 和 场景 将 始终 可 访问 ,因此 无 须 再 次 指定 。 

EI": “分隔 每 个 目录 (Windows 环境 下 使 用 * ;”)。 

2. OPENRAVE_DATABASE 

用 于 加 载 由 OpenRA VE 数据 库 系 统 创 建 的 数据 库 文件 的 搜索 路 径 。 数据 库 用 于 存储 
有 关机 器 人 、 目 标 对 象 和 传感器 的 有 用 信息 和 统计 信息 ,这 些 信息 和 统计 信息 需要 很 长 时 间 
才能 预先 计算 或 同步 到 真实 数据 。 写 人 时 ,使 用 第 一 个 有 效 目 录 。 如 果 未 设置 环境 变量 , 则 
使 用 $ OPENRAVE_HOME。 

fH": "分隔 每 个 目录 (Windows 环境 下 使 用 * ;”)。 

3. OPENRAVE_HOME 

用 于 设置 OpenRAVE 本 地 缓存 和 日 志文 件 的 目录 。 默 认 目 录 为 $ HOME/. OpenRAVE 。 

4. OPENRAVE PLUGINS 

在 启动 时 ,OpenRAVE 搜索 这 些 目录 中 的 每 个 共享 对 象 /dll 插件 并 加 载 它 们 。 默 认 插 
件 总 是 加 载 , 因 此 不 需要 再 次 包含 它们 。 

使 用 * : ”分 隔 每 个 目录 (Windows 环境 下 使 用 * ;”)。 


5.4 OpenRAVE 运用 与 展望 


5.4.1 OpenRAVE 的 运用 项 目 举例 


1. 5 ROS(Robot Operation System) 的 应 用 

任何 机 器 人 系统 都 应 该 通过 视觉 反馈 、 传 感 器 回路 和 更 高 层次 的 推理 来 处 理 自主 操纵 。 

OpenRAVE 可 以 在 许多 不 同 的 场景 中 使 用 。 

(1) 有 一 个 OpenRAVE 实例 进行 规划 ,用 户 想 要 所 有 的 控制 器 和 传感器 馈送 信息 给 
。 我 们 称 为 Master。 


n 
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(2) 在 Master 之 外 ,有 OpenRA VE 实例 . 它 包 装 硬件 和 仿真 控制 器 ,生成 仿真 的 传 感 
器 数据 。 这 些 实例 发 布 到 ROS 网 络 ,通常 馈送 到 MasterOpenRAVE。 

1) OpenRAVE 插件 连接 到 ROS 

有 几 个 OpenRAVE/ros 插件 ,在 内 部 创建 节点 和 发 布 和 获取 消息 。 这 些 包 可 以 在 jsk- 
ros-pkg(https://sourceforge. net/projects/jsk-ros-pkg/? source 王 navbar) 中 找到 。 
可 以 通过 ROS 网 络 向 OpenRAVE 发 送 命令 。 在 operaveros tutorials 
中 有 部 分 内 容 (http://wiki. ros. org/OpenRAVEros tutorials) 。 

OpenRAVE sensors 获取 ROS 消息 以 传输 传感器 数据 到 OpenRAVE( 由 Master 
加 载 的 数据 ) 。 


operave robot control 


operaveros 


用 于 通过 ROS 网 络 来 控制 机 器 人 的 简单 回话 窗口 ,operave 
是 底层 的 客户 端 ,注意 在 lib 文件 夹 中 有 一 个 librobot control. so OpenRAVE 插件 ,是 主 控 
插件 。 

schunk motion controllers 一 一 连接 到 Schunk 硬件 接口 并 给 出 指令 来 控制 机 器 人 。 

orrosplanning 一 一 额外 插件 ,用 来 读 取 传 感 数据 并 把 它 展示 在 OpenRA VE 中 。 例 如 ， 
如 果 有 一 个 节点 plublishingcheckerboard | detector/ObjectDetection 消息 , 则 可 以 使 用 
ObjectTransform OpenRAVESensorSystem 接口 在 环境 中 显示 对 象 。 

2) 组 件 部 分 内 容 

ControllingRobots 一 一 通过 ROS 使 用 OpenRA VE 控制 机 器 人 。 

C X BE HE. http://OpenRAVE. programmingvision. com/wiki/index. php/ROS; 
ControllingRobots. ) 

Sensors 一 一 通过 ROS 发 布 OpenRAVE 传感器 数据 。 

(参考 链接 : http://OpenRAVE. programmingvision. com/wiki/index. php/ROS: 
sensors. ) 

ROS:Object Detection 一 一 简单 的 对 象 检测 和 将 对 象 插入 到 环境 中 。 

(参考 链接 : http://OpenRAVE. programmingvision, com/wiki/index. php/ROS: 
Object_Detection. ) 

2. 其 他 

例如 ,在 Intel Research Pittsburgh 中 的 Personal Robotics。 

OpenGRASP 一 一 OpenGRASP 是 一 个 用 于 抓 取 和 灵活 操作 的 开源 仿真 工具 包 。 它 支 
持 创建 和 添加 新 功能 以 及 集成 了 现 有 的 和 广泛 使 用 的 技术 和 标准 。 

Modular Robots OpenMR 是 一 个 OpenRAVE 模块 化 机 器 人 插件 ,用 于 模拟 模块 
化 机 器 人 的 运动 。 

此 外 , 还 有 : Constrained Manipulation Planning Suite ( COoMPS), Planning Arm 
System, RTC-OpenRAVE, Open Probabilistic Roadmap Planning, SmartSoft Toolchain, 
Fawkes Robotics, Arm Model-Based Hierarchical Planner, Box Packing Robot, Lego 
Project, MiniHubo. 
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5.4.2 OpenRAVE 的 展望 


OpenRAVE 中 使 用 传感器 反馈 模拟 6 自由 度 机 器 人 手臂 的 性 能 变 得 足以 胜任 ,并 且 足 
够 稳定 地 处 理 机 器 人 模拟 ,包括 对 象 抓 取 求解 逆 运 动 学 `. 运 动 规划 等 。 

在 OpenRAVE 使 用 其 物理 引擎 的 帮助 下 ,可 以 在 虚拟 环境 中 模拟 更 多 任务 ,这 与 在 真 
实 世 界 使 用 其 物理 引擎 非常 相似 (仿真 度 非常 高 )。 利 用 传感器 反馈 ,模拟 器 可 以 执行 复杂 
的 动作 ,例如 以 更 高 的 成 功率 抓 取 不 规则 或 复杂 的 物体 。 与 对 象 抓 取 相 关 的 研究 可 以 使 用 
OpenRAVE 进一步 研究 。 此 外 ,OpenRAVE 会 更 加 实用 ,如 果 OpenRAVE 中 的 轨迹 规划 
器 的 计算 结果 可 以 把 每 个 关节 的 命令 输出 给 各 种 机 器 人 ,让 模拟 器 指导 它 在 现实 世界 中 执 
行 任务 。 如 果 是 这 样 ,许多 机 器 人 研究 在 OpenRAVE 的 帮助 下 将 非常 高 效 和 简单 。 

目前 ,OpenRAVE 没有 能 力 计算 对 象 和 机 器 人 手 之 间 的 接触 力 。 如 果 解 决 了 判断 接触 
力 是 否 足够 大 以 抓 握 物 体 , 模 拟 器 将 会 变 得 非常 适合 于 对 象 抓 取 的 机 器 人 模拟 研究 。 

OpenRAVE 为 在 现实 世界 的 机 器 人 应 用 中 测试 ,开发 和 部 署 运动 规划 算法 提供 了 一 个 
环境 。 主 要 关注 与 运动 规划 相关 的 运动 学 和 几何 信息 的 模拟 和 分 析 。OpenRAVE 独立 的 
性 质 允 许 轻松 集成 到 现 有 的 机 器 人 系统 中 。 它 提供 了 许多 命令 行 工具 来 使 用 机 器 人 和 规划 
器 ,并 且 运 行 时 核心 足够 小 ,可 以 在 控制 器 和 更 大 的 框架 内 使 用 。 


本 章 小 结 


本 章 主要 介绍 OpenRAVE 的 基础 知识 和 应 用 。OpenRAVE 主要 应 用 于 机 器 人 的 运动 
规划 ,在 这 里 应 当 与 前 两 个 功能 全 面 的 机 器 人 仿真 软件 区 别 开 来 ,重点 关注 OpenRAVE 的 
主要 特性 和 用 途 。 
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机 器 人 操作 系统 的 概述 


ROS 的 诞生 


随 着 机 器 人 技术 的 快速 发 展 , 对 机 器 人 代码 的 可 重用 性 和 模块 化 的 需 
KRKA, Æ 2010 年 ,Willow Garage 机 器 人 公司 发 布 的 一 个 开源 的 
操作 系统 称 为 Robot Operating System( 机 器 人 操作 系统 ), 简 称 ROS, 
随 着 这 个 操作 系统 的 推出 ,在 机 器 人 的 研究 领域 沪 快 就 引发 了 人 们 对 
ROS 的 学 习 和 应 用 热潮 。 为 什么 ROS 具有 如 此 的 魅力 ? 下面 将 对 ROS 
的 功能 和 应 用 进行 介绍 。 

在 ROS 的 官方 网 站 (http://wiki. ros.org) 中 ,关于 ROS 的 简要 介 
绍 如 下 : ROS 提供 一 系列 程序 库 和 工具 以 帮助 软件 开发 者 创建 机 器 人 应 
用 软件 。 同 时 , 它 还 提供 了 硬件 抽象 .设备 驱动 .函数 库 、 可 视 化 工具 消息 
传递 和 软件 包 管 理 等 诸多 功能 。 

然而 对 于 初学 者 而 言 , 以 上 的 简单 介绍 殷 难 让 他 们 深入 了 解 ROS 的 
功能 ,下 面 进行 更 深 一 步 的 解析 。 

首先 ROS 提供 了 一 个 标准 的 操作 系统 ,包括 硬件 抽 和 象 \ 底层 设备 控 
制 、 常 用 函数 的 实现 .进程 间 消 息 传递 和 包 管 理 。 基 于 这 个 标准 系统 ,用 户 
可 以 容易 地 构建 和 编写 自 开 发 代码 所 需要 的 工具 和 库 。 并 且 , 自 开发 的 代 
码 也 可 以 在 多 合计 算 机 共享 使 用 ,大 大 提高 了 自 开 发 代码 的 可 移植 性 。 
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ROS 设计 的 主要 目标 是 在 机 器 人 的 研发 中 提高 代码 的 重复 利用 率 。ROS 是 一 个 
分 布 式 的 处 理 框架 ,这 使 得 可 执行 文件 可 以 被 单独 设计 ,而 且 具 有 松散 耦合 的 特点 。 这 
些 过 程 可 以 被 封装 成 数据 包 和 栈 , 因 此 能 够 被 进一步 地 发 布 和 共享 。ROS 也 支持 联合 
系统 的 代码 库 , 因 此 协作 也 可 以 是 分 布 式 的 制定 独立 的 决策 和 实施 。 上 面 所 有 的 这 些 
功能 可 以 通过 基本 的 ROS 工具 来 实现 。 

ROS 本 身 昌 然 并 不 是 一 个 实时 框架 ,但 是 它 可 以 将 ROS 和 实时 的 代码 进行 集成 。 
典型 的 集成 例子 如 Willow Garage PR2, 它 使 用 一 个 称 为 pr2etherCAT 的 系统 ,这 个 
系统 可 以 在 一 个 实时 的 进程 中 传输 ROS 的 消息 。 此 外 ,ROS 还 具有 许多 可 用 的 实时 工 
具 包 。 

目前 ,ROS 主要 运行 在 UNIX 平台 上 ,有 关 ROS 的 软件 主要 是 在 Ubuntu 和 Mac 
OS X 系 统 上 测试 的 。 但 是 ,通过 ROS community, 它 也 支持 Fedora, Gentoo, Arch 
Linux 和 其 他 Linux 平台 。 


ROS 的 主要 特性 
ROS 的 运行 架构 是 一 种 使 用 ROS 通信 模块 实现 模块 间 P2P 654238 S 65 Jm £8 i jk 
的 处 理 架 构 , 它 执行 若干 种 类 型 的 通信 ,包括 基于 服务 的 同步 远程 过 程 调 用 (RPC) 通 


信 、 基 于 Topic 的 异步 数据 流通 信 , 还 有 参数 服务 器 上 的 数据 存储 。 

ROS 的 主要 特性 如 下 

1. 分 布 式 通信 

ROS 是 一 个 包括 了 一 系列 进程 的 系统 ,这 些 进 程 存在 于 许多 不 同 的 主机 中 ,在 运行 
过 程 中 相互 之 间 通 过 end-topology 通信 。ROS 的 点 对 点 设计 和 节点 管理 机 制 能 够 分 
散 计 算 机 视觉 和 语言 识别 等 功能 带 来 的 实时 计算 超 负 荷 压 力 ,改善 这 些 问 题 。 尽 管 这 
些 基 于 中 央 服 务 器 的 软件 框架 能 够 实现 多 进程 和 多 主机 带 来 的 好 处 ,但 是 当 计 算 机 是 

过 不 同 网 络 进行 连接 时 ,中 央 数 据 服 务 器 将 出 现 问 题 。 

2. 语言 独立 性 

在 进行 程序 设计 的 过 程 中 ,大 部 分 程序 员 倾 向 于 选择 他 们 所 习惯 \ 擅 长 的 语言 。 针 
对 这 种 情况 ,ROS 设计 了 一 个 语言 中 立 框 架 , 因 此 能 够 支持 与 不 同 的 计算 机 语言 ,如 C 
++ ,Python、Octave、LISO 以 及 其 他 一 系列 编程 语言 。ROS 的 特殊 之 处 主要 在 于 它 的 
消息 通信 层 一 一 端 对 端的 连接 和 配置 利用 XML-RPC 机 制 进行 实现 ,同时 XML-RPC 
也 包含 了 大 多 数 主 要 语言 的 合理 实现 描述 。ROS 的 设计 者 想 要 ROS 更 自然 地 使 用 多 
种 语言 ,更 符合 各 种 语言 的 语法 规则 ,而 不 只 是 基于 C 语言 的 接口 。 在 某 些 情况 下 , 利 
用 已 经 存在 的 库 封 装 后 支持 更 多 新 的 语言 是 女方 便 的 ,例如 Octave 的 客户 端 就 是 通 
C++ 的 封装 库 进 行 实现 的 。 为 了 支持 交叉 语言 ,ROS 利用 了 简单 、 独 立 的 接口 定义 节 
去 描述 模块 之 间 的 消息 传送 。 接 口 定义 语言 使 用 了 简短 的 文本 去 描述 每 条 消息 的 
构 , 同 时 也 允许 消息 的 合成 。 


m o} E 
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3. 简便 的 代码 复 用 

大 多 数 已 经 存在 的 机 器 人 软件 工程 都 包含 了 可 以 在 工程 外 重复 使 用 的 驱动 和 算 
法 。 不 幸 的 是 ,由 于 多 方面 的 原因 ,大 部 分 代码 的 中 间 层 都 过 于 混乱 ,以 至 于 殷 难 从 中 
提取 出 它 的 功能 ,进而 殷 难 把 它们 从 原型 中 提取 出 来 ,并 应 用 到 其 他 方面 。 为 了 改善 这 
种 状况 ,我 们 鼓励 将 所 有 的 驱动 和 算法 逐渐 发 展 成 为 和 ROS 没有 依赖 性 单独 的 库 。 基 
F ROS 建立 的 系统 具有 模块 化 的 特点 ,各 模块 中 的 代码 可 以 单独 编译 ,而 且 编 译 使 用 
的 CMake 工具 使 它 殷 容易 地 就 实现 精简 的 理念 。ROS 基本 上 已 经 将 复杂 的 代码 封装 
在 库 里 ,并 且 创 建 了 一 些小 的 应 用 程序 用 来 显示 库 的 功能 。 这 样 就 允许 了 对 简单 的 代 
码 超越 原型 进行 移植 和 重新 使 用 。 作 为 一 种 新 加 入 的 优势 ,单元 测试 的 代码 在 库 分 散 
也 变 得 非常 容易 ,一 个 单独 的 测试 程序 可 以 测试 库 中 的 殷 多 特点 。 

ROS 利用 了 殷 多 已 经 存在 的 开源 项 目的 代码 ,例如 ,从 Player 项 目 中 借鉴 了 驱动 、 
运动 控制 和 仿真 方面 的 代码 .从 OpenCV 中 借鉴 了 视觉 算法 方面 的 代码 .从 OpenRAVE. 
借鉴 了 规划 算法 的 内 容 。 在 每 一 个 实例 中 ,ROS 都 用 来 显示 多 种 多 样 的 配置 选项 以 及 
和 各 软件 之 间 进 行 数据 通信 ,也 同时 允许 对 它们 进行 注 小 的 包装 和 改动 。ROS 可 以 不 
断 地 从 社区 维护 中 进行 升级 ,包括 从 其 他 的 软件 库 、 应 用 补丁 中 升级 ROS 的 源 代码 。 

4. 精致 的 内 核 设计 

为 了 管理 复杂 的 ROS 软件 框架 ,设计 者 并 不 是 通过 构建 一 个 庞大 的 开发 和 运行 环 
境 , 而 是 利用 了 大 量 的 小 工具 去 编译 和 运行 多 种 多 样 的 ROS 组 建 来 设计 系统 内 核 。 这 
些 工 具 查 任 了 各 种 各 样 的 任务 ,例如 ,组 织 源 代码 的 结构 获取 和 配置 参数 .形象 化 端 对 
端的 折 扑 连接 .测量 频带 使 用 宽度 、 生 动 的 描绘 信息 数据 .自动 生成 文档 等 。 尽 管 我 们 
已 经 通过 像 全 局 时 钟 和 控制 器 模块 的 记录 器 来 测试 核心 服务 ,但 是 还 是 希望 能 把 所 有 
的 代码 模块 化 ,因为 在 效率 上 的 损失 远 远 是 稳定 性 和 管理 的 复杂 性 无 法 弥补 的 。 

5. 免费 且 开 源 的 代码 

ROS 所 有 的 源 代 码 都 是 公开 发 布 的 。 这 将 兴 定 促进 ROS 软件 各 层次 的 调试 ,不 断 
地 改正 错误 。 昌 然 像 Microsoft Robotics Studio 和 Webots 这 样 的 非 开源 软件 也 有 
瀛 多 值得 鞠 美 的 属性 ,但 是 我 们 认为 一 个 开源 的 平台 也 是 无 可 替代 的 。 当 硬件 和 各 层 
次 的 软件 同时 设计 和 调试 的 时 候 , 这 一 点 是 尤为 明显 的 。 


ROS 的 层次 概念 


如 果 根 据 ROS 系统 代码 的 维护 者 和 分 布 来 分 层 ,ROS 主要 由 以 下 两 大 部 分 构成 ; 

(1) main; 核心 部 分 ,主要 由 Willow Garage 公司 和 一 些 开发 者 设计 提供 以 及 维 
护 。 它 提供 了 一 些 分 布 式 计算 的 基本 工具 以 及 编写 整个 ROS 的 核心 部 分 的 程序 。 

(2) universe, 全 球 范围 的 代码 ,有 不 同 国家 的 ROS 社区 组 织 开发 和 维护 。 一 种 
是 库 的 代码 ,如 OpenCV、PCL 等 ; 库 的 上 一 层 是 从 功能 角度 提供 的 代码 ,如 人 脸 识 别 ， 
它们 通过 调用 下 层 的 库 来 实现 功能 ; 最 上 层 是 应 用 级 的 代码 ,让 机 器 人 完成 某 一 确定 的 
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功能 。 
一 般 来 说 ,学 界 经 常 从 另 一 个 角度 对 ROS 分 级 ,主要 分 为 三 个 级 别 ; 计算 图 级 
(ROS Computation Graph Level)、 文 件 系 统 级 (ROS Filesystem Level)、 和 社区 级 


(ROS Community Level) 。 分 别 介绍 如 下 。 

1. 计算 图 级 

计算 图 是 ROS 处 理 数 据 的 一 种 点 对 点 的 网 络 形 式 。 程 序 运 行 时 ,所 有 进程 以 及 它 
们 所 进行 的 数据 处 理 , 将 会 通过 一 种 点 对 点 的 网 络 形式 表现 出 来 。 这 一 级 主要 包括 几 
个 重要 概念 ; 节点 (node) .消息 (message) .主题 (topic)、 服务 (service) 。 

1) 节点 

节点 就 是 一 些 执行 运算 任务 的 进程 。ROS 具有 的 规模 可 增长 特性 是 利用 代码 模块 
化 来 实现 的 : 一 个 典型 的 系统 就 是 由 很 多 节点 组 成 的 。 在 这 里 ,节点 也 可 以 被 称 为 “ 软 
件 模 块 ”。 使 用 "节点 ”使 得 基于 ROS 的 系统 在 运行 的 时 侠 更 加 形象 化 : 当 许多 节点 同 
时 运行 时 ,可 以 殷 方 便 地 将 端 对 端的 通信 绘制 成 一 个 图 表 , 在 这 个 图 表 中 ,进程 就 是 图 
中 的 节点 ,而 端 对 端的 连接 关系 就 是 其 中 弧 线 连接 。 

2) 消息 

节点 之 间 是 通过 传送 消息 进行 通信 的 。 每 一 个 消息 都 是 一 个 严格 的 数据 结构 。 这 
个 数据 结构 支持 标准 的 数据 类 型 (如 整 型 \ 浮 点 型 \ 布 尔 型 等 ), 也 支持 这 些 类 型 所 组 成 
的 数组 类 型 ,并 且 它 也 支持 包含 任意 数据 类 型 的 岩 套 结构 和 数组 (类 似 于 C 语言 的 结 
构 体 )。 

3) 主题 

主题 是 指 消 息 以 一 种 发 布 (Publish)/ 订 阅 (Subscribe) 的 方式 传递 。 一 个 节点 可 
以 在 一 个 给 定 的 主题 中 发 布 消 息 , 也 可 以 关注 ,订阅 某 个 主题 特定 类 型 的 数据 。 可 能 同 
时 有 多 个 节点 发 布 或 者 订阅 同一 个 主题 的 消息 。 总 的 来 说 ,发 布 者 和 订阅 者 不 了 解 波 
此 的 存在 。 

4) 服务 

虽然 基于 主题 的 发 布 /订阅 模型 是 奶 灵 活 的 通信 模式 ,但 是 它 广 播 式 的 路 径 规 划 并 
不 适合 于 可 简化 节点 设计 的 同步 传输 模式 。 在 ROS 中 ,一 个 服务 代表 着 用 一 个 字符 串 
和 一 对 严格 规范 的 消息 定义 :一 个 用 于 请 求 , 另 一 个 用 于 回应 。 这 类 似 于 Web 服务 器 ， 
Web 服务 器 是 由 URIS 定义 的 ,同时 带 有 完整 定义 类 型 的 请 求 和 回复 文档 。 需 要 注意 
的 是 ,一 个 节点 可 以 以 任意 独 有 的 名 字 广 播 一 个 服务 : 只 有 一 个 服务 可 以 称 之 为 “分 类 
象征 ”, 例 如 ,任意 一 个 给 定 的 URI 地 址 只 能 有 一 个 Web 服务 器 。 

2. 文件 系统 级 

ROS 文件 系统 级 指 的 是 在 硬盘 上 面 查看 的 ROS 源 代码 的 组 织 形 式 。ROS 中 有 无 
数 的 节点 ,消息 ,服务 、 工 具 和 库 文件 ,需要 有 效 的 结构 去 管理 这 些 代码 。 在 ROS 的 文 
件 系 统 级 ,有 以 下 两 个 重要 概念 : 包 (Package) 与 堆 (Stack)。 
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DE 

ROS 的 软件 以 包 (Package) 的 方式 组 织 起 来 。Package 包含 节点 、ROS 依赖 库 、 
数据 套 、 配 置 文件 .第 三 方 软件 ,或 者 任何 其 他 还 辑 构成 。 包 的 目标 是 提供 一 种 易于 使 
用 的 结构 ,以 便于 软件 的 重复 使 用 。 

2) 堆 

堆 是 包 的 集合 , 它 提供 一 个 完整 的 功能 , 像 “navigation stack", Stack 与 版 本 号 
关联 ,同时 也 是 如 何 发 行 ROS 软件 方式 的 关键 。 

ROS 是 一 种 分 布 式 处 理 框架 。 这 使 可 执行 文件 能 被 单独 设计 ,并 且 在 运行 时 松散 
耦合 。 这 些 过 程 可 以 封装 到 包 和 堆 中 ,以 便于 共享 和 分 发 。 

Manifests(manifest.xml): 提供 关于 Package 元 数据 ,包括 它 的 许可 信息 和 
Package 之 间 依 赖 关系 ,以 及 语言 特性 信息 (如 编译 优化 参数 )。 

Stack manifests(stack.xml), 提供 关于 Stack 元 数据 ,包括 它 的 许可 信息 和 
Stack 之 间 依 赖 关系 。 

3. 社区 级 

ROS 的 社区 级 概念 是 在 ROS 网 络 上 进行 代码 发 布 的 一 种 表现 形式 。 

代码 库 的 联合 系统 使 得 协作 亦 能 被 分 发 。 这 种 从 文件 系统 级 别 到 社区 一 级 的 设计 
让 不 同 开发 者 独立 地 发 展 和 实施 工作 成 为 可 能 。 正 是 因为 这 种 分 布 式 的 结构 ,使 得 
ROS 迅速 发 展 ,软件 仓库 中 包 的 数量 以 指数 级 增加 。 
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6.1 ROS 的 安装 与 测试 


6.1.1 虚拟 机 与 Ubuntu 的 安装 


虚拟 机 (Virtual Machine) 指 通过 软件 仿真 一 个 完整 的 计算 机 系统 , 它 可 以 使 用 本 机 的 
硬件 系统 ,但 同时 与 主要 的 计算 机 系统 完全 隔离 。 本 章节 将 展示 如 何在 虚拟 机 中 安装 
Ubuntu 14. 04 操作 系统 以 及 如 何在 Ubuntu 中 使 用 ROS, 下 面 以 虚拟 机 软件 VMware 
Workstation 10 为 例 向 读者 介绍 。 

(1) 在 Ubuntu 的 网 站 (https://www. ubuntu. com/download) 下 载 后 级 名 为 iso 的 
Ubuntu 镜像 文件 ,注意 相应 的 版 本 和 计算 机 位 数 。 

(2) 启动 VMware Workstation, 并 选择 创建 虚拟 机 ,进入 新 建 虚拟 机 向 导 界 面 ,如 图 6-1 
所 示 。 


连接 远程 服务 器 
创建 新 的 虚拟 机 在 二 得 服务 串 上 可 看 和 管理 虑 拟 机 
虚拟 化 物理 机 
从 现 有 物理 机 创建 起 拟 机 . 
打开 虚拟 机 软件 更 新 
J 检查 VMware Workstation 


图 6-1 虚拟 机 向 导 界 面 
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(3) 如 图 6-2 所 示 ,选择 “典型 "的 配置 ,安装 来 源 选 择 ,并 选择 “ 稍 后 安装 操作 系统 ”, 然 
后 选择 “Linux” 和 相应 的 版 本 。 


图 6-2 虚拟 机 安装 界面 一 


CD. 命名 虚拟 机 的 名 称 ,并 选择 好 相应 的 安装 位 置 ; 如 图 6-3 所 示 , 然 后 根据 自己 的 需 
求 指定 磁盘 的 容量 ,从 而 完成 了 虚拟 机 的 创建 。 此 时 ,在 “我 的 计算 机 ”一 栏 可 以 显示 出 我 们 
已 创建 的 虚拟 机 。 


Lo uj 
KRAAS? 


和 


BARA NGBXS): 200 图 
PHRI Ubuntu 64 位 的 建议 大 小 : 20 GB 


C hide ioo AHO) 


O HERRARNA) 
nM SIUIBENEC HEELS EIEEEIERUR. (SSTICSSPEHEC MUR 


图 6-3 虚拟 机 安装 界面 二 
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(5) 如 图 6-4 所 示 ,虚拟 机 的 设置 ,在 硬件 的 CD/DVD 一 栏 中 选择 使 用 已 下 载 的 ISO 
xit. 


Ratt 
Digi 
T BEER CO) 


E: 
© PIN): 


Bitim 
© 使 用 SOMEAH): 


F:\ubuntu-14.04.2-desktop « Cata) 
[ao | 


图 6-4 虚拟 机 安装 界面 三 
(6) 设置 完 开启 虚拟 机 ,从 而 进入 Ubuntu 的 安装 界面 。 如 图 6-5 所 示 , 根 据 自 己 的 需 
求 选 择 相应 的 安装 设置 ,完成 Ubuntu 的 安装 。 
需要 说 明 的 是 ,使 用 VMware Workstation 安装 Ubuntu 不 局 限于 以 上 的 方法 。 使 用 


这 种 方法 的 好 处 是 在 安装 的 过 程 中 无 须 联网 下 载 过 多 的 系统 更 新 ,节省 时 间 。 安 装 完成 后 ， 
在 相应 的 系统 安装 位 置 生成 了 相应 的 虚拟 机 系统 文件 。 


6.1.2 ROS 的 安装 


ROS 有 三 个 版 本 ,下 面 只 介绍 ROS 的 Indigo 版 本 在 Ubuntu 14. 04 安装 的 详细 步骤 。 

(D 配置 Ubuntu 的 存储 库 。 使 它 允 许 *restricted” universe" fll" multiverse", — At Æ 
说 ,它们 已 经 被 正确 地 配置 ,只 需要 确认 以 上 的 配置 。 

© 添加 ROS 的 源 列表 。 打 开 Ubuntu 的 终端 窗口 ,输入 以 下 指令 添加 ROS 的 源 列表 : 


$ sudo sh -c 'echo "deb http://packages. ros. org/ros/ubuntu trusty main" > /etc/apt/sources 
. list. d/ros - latest. list' 
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varat 


opidi 


aa 


milny 


ana 试用 Ubuntu 安装 Ubuntu 


您 可 以 直接 从 此 CD i£ Ubuntu ,而 不 用 对 您 的 电脑 作 任何 更 改 。 


如 果 您 已 经 准备 完毕 ， 您 可 以 与 现 有 系统 并 存 (sci MIX) 方式 将 Ubuntu 安装 到 您 的 电 
脑 上 。 此 过 程 无 需 耗 时 太 久 。 


"x (um) 
日 本 语 您 可 以 阅读 一 下 发 行 注 


图 6-5 Ubuntu 的 安装 界面 
@ 设置 授权 密 钥 ,如 下 : 
$ wget http://packages.ros.org/ros.key -0 - | sudo apt- key add — 
@ 更 新 软件 包 , 如 下 : 


$ sudo apt- get update 


E 整 安装 ROS Indigo 版 本 ,如 下 :; 

$ sudo apt- get install ros- indigo- desktop- full 

注 : 在 安装 的 过 程 中 可 能 会 得 到 一 个 提示 “hddtemp”, 可 以 输入 “No”, 继 续 下 一 步 的 
安装 。 

© 初始 化 rosdep 如 下 : 


$ sudo rosdep init 
$ rosdep update 
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D 安装 rosinstall 如 下 : 
$ sudo apt- get install python- rosinstall 


创建 开发 工作 空间 时 的 步骤 如 下 : 
O 创建 ROS 工作 空间 如 下 : 


$ mkdir - p —/ros ws/src 


© 设置 ROS 源 如 下 : 


$ source /opt/ros/indigo/setup. bash 


O 创建 和 安装 如 下 : 


$ cd —/ros ws 
$ catkin make 
$ catkin make install 


6.1.3 turtlesim 例子 的 测试 


当 安 装 好 ROS 时 ,可 使 用 turtlesim 仿真 器 中 一 个 简单 的 例子 进行 测试 。 首 先 打开 三 
个 不 同 的 终端 ,分 别 在 窗口 中 输入 以 下 三 个 命令 ,并 逐一 执行 : 


对 于 终端 一 : 
roscore 
对 于 终端 二 ， 


rosrun turtlesim turtlesim node 


对 于 终端 三 : 
rosrun turtlesim turtle teleop key 


执行 3 个 指令 后 ,将 弹出 以 下 的 一 个 名 为 “TurtleSim” 的 仿真 窗口 ,在 图 像 界面 中 有 一 
个 海龟 。 此 时 ,切换 到 终端 三 的 界面 中 ,通过 键盘 的 方向 键 可 控制 海龟 进行 运动 。 其 中 ， 
“>” 和 “< 一 "改变 海龟 的 方向 ,和 ”和 “y "控制 海龟 的 前 进 与 后 退 。 海 龟 在 移动 过 程 中 ,背景 
界面 会 记录 有 疏 行 的 轨迹 ,如 图 6-6 所 示 。 
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图 6-6 仿真 实例 : 控制 海龟 移动 


6.2 ROS 的 基本 概念 与 命令 


6.2.1 程序 包 (packages) 

程序 包 , 简 称 为 包 , 是 ROS 应 用 程序 代码 的 组 织 单元 。 程 序 包 中 有 一 个 重要 的 清单 ， 
Manifests 清单 (package. xml) 。 它 提供 有 关 软 件 包 的 元 数据 ,包括 其 名 称 版本、 描述 .许可 
证 信息 ,依赖 关系 和 其 他 元 信息 (如 导出 的 软件 包 )。 

下 面 介绍 程序 包 中 常用 的 操作 命令 。 

1. 查看 软件 包 列 表 


rospack list 
说 明 : 运行 该 命令 将 会 获取 系统 中 已 安装 的 所 有 ROS 程序 包 , 并 以 列表 的 形式 呈现 在 


窗口 下 面 。 
例如 : 输入 命令 : 


rospack list 
运行 结果 为 : 


actionlib /opt/ros/indigo/share/actionlib 
actionlib msgs /opt/ros/indigo/share/actionlib msgs 
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actionlib tutorials /opt/ros/indigo/share/actionlib tutorials 


其 中 ,actionlib ,actionlib_msgs,actionlib_tutorials 为 程序 包 的 名 称 , 后 面 为 程序 包 所 在 的 
H3. 
2. 查找 某 程序 包 所 在 的 目录 


rospack find + package - name( 程 序 包 的 名 字 ) 


说 明 : 运行 该 命令 能 够 获取 某 个 程序 包 所 在 的 目录 。 在 这 里 ,给 读者 们 提供 一 个 常用 
的 小 技巧 ,因为 程序 包 的 路 径 以 及 名 称 通 常 都 比较 元 长 难 记 , 因 此 ,假如 读者 不 能 清楚 记得 
某 个 程序 包 的 名 称 时 ,可 以 通过 tab 命令 补 全 的 方法 获取 完整 的 程序 包 名 称 : 即 输 入 名 称 
开头 几 个 字母 后 ,通过 按 下 两 次 Tab 键 , 便 可 获取 所 有 以 这 些 字母 开头 的 程序 包 。 

例如 :〈1) 如 查找 程序 包 camera calibration 的 目录 ,输入 命令 : 


rospack find actionlib 
运行 结果 为 : 
/opt/ros/indigo/share/actionlib 


(2) 输入 rospack find actionlib 后 ,可 以 通过 按 两 次 Tab 键 ,获取 以 “actionlib” 开 头 的 
程序 包 , 运 行 结果 如 下 : 


actionlib actionlib msgs actionlib tutorials 
3. 查看 程序 包 


rosls package - name 


说 明 : 运行 该 命令 能 够 获取 该 程序 包 目 录 下 的 所 有 文件 。 

例如 : 通过 前 面 的 例子 可 以 知道 ,actionlib 的 目录 为 : opt/ros/indigo/share/actionlib. 
如 图 6-7 所 示 ,通过 查找 该 目录 可 得 到 该 目录 下 有 以 下 4 个 文件 ( 夹 ), 用 以 下 命令 进行 验 
证 ,输入 命令 。 


rosls actionlib 
运行 结果 为 : 


action cmake msg package. xml 
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- d -d a 
action cmake msg package.xml 


图 6-7 程序 包 中 的 文件 夹 
4. 访问 程序 包 


roscd package - name 


说 明 : 通过 运行 该 命令 可 以 访问 某 程序 包 的 目录 。 
例如 ,输入 如 下 命令 : 


roscd actionlib 
运行 结果 为 : 
/opt/ros/indigo/share/actionlib $ 


它 将 当前 的 目录 切换 到 actionlib 的 目录 ,从 而 可 以 对 该 目录 中 的 文件 进行 下 一 步 
操作 。 


6.2.2 节点 (Nodes) 和 节点 管理 器 (Master) 


节点 : 节点 是 执行 计算 的 进程 。ROS 设计 为 以 节点 为 单位 进行 模块 化 ; 机 器 人 控制 系 
统 通常 包括 许多 节点 。 例 如 ,一 个 节点 控制 激光 测 距 仪 ,一 个 节点 控制 车 轮 电动 机 ,一 个 节 
点 执行 定位 ,一 个 节点 执行 路 径 规划 ,一 个 节点 提供 系统 的 图 形 视图 等 。ROS 的 节点 管理 
器 提供 名 称 注册 和 查找 计算 图 形 的 其 余部 分 。 没 有 节点 管理 器 ,节点 将 无 法 找到 彼此 ,执行 
交换 消息 或 调用 服务 。 
质 上 是 一 个 可 执行 文件 , 它 可 以 通过 ROS 与 其 他 节点 进行 通信 。 而 在 ROS 中 ， 
实现 节点 与 节点 之 间 的 关键 部 分 为 节点 管理 器 。 运 行 ROS 的 工作 流程 是 : 在 一 个 终端 中 
启动 节点 管理 器 ,然后 再 打开 其 他 终端 运行 其 他 程序 。 例 如 ,前 面 的 海龟 仿真 例子 , 先 在 一 
个 终端 中 运行 roscore, 然 后 在 另外 两 个 终端 分 别 运行 相关 的 程序 。 

1. 启动 节点 管理 器 


roscore 


说 明 : 当 运 行 完 所 有 与 ROS 相关 的 程序 时 ,可 通过 Ctrl 十 C 组 合 键 停止 节点 管理 器 。 
2. 启动 节点 


rosrun package - name executable- name 
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例如 : 海龟 仿 真 在 另外 两 个 终端 运行 的 命令 为 : 


| 


其 中 ,turtlesim 为 程序 包 名 称 ,turtlesim_node 和 turtle teleop key 为 可 执行 文件 名 。 
3. 查看 节点 列表 


说 明 : 运行 该 命令 能 够 获取 当前 时 间 内 正在 运行 的 节点 信息 。 
例如 : 运行 海龟 仿真 时 ,可 以 看 到 3 个 节点 的 列表 : 


4. 查看 节点 


说 明 : 运行 该 命令 能 够 获得 某 个 节点 的 信息 。 
例如 : 查看 turtlesim 的 信息 ,输入 命令 : 


运行 结果 为 : 
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* /clear 
* /turtlel/set pen 
* /turtlel/teleport relative 
* /kill 


说 明 : 能 够 获取 的 3 Fifi EA Publications, Subscriptions 和 Services. 
5. 终止 节点 


rosnode kill node - name 


说 明 : 运行 该 命令 能 够 终止 某 个 节点 ,一 般 情况 下 对 其 他 节点 不 会 有 影响 。 
例如 ,输入 命令 : 


rosnode kill turtlesim 


运行 结果 为 : 


killing /turtlesim 
killed 


6.2.3 消息 (Messages) 和 主题 CTopics) 

前 文中 提 过 ,消息 只 是 一 个 数据 结构 ,包括 类 型 字段 。 支 持 标准 基本 类 型 (整数 、 浮 点 、 
布尔 等 ) ,以 及 原始 类 型 数组 。 

主题 : 消息 通过 具有 发 布 /订阅 语义 的 传输 系统 进行 交换 。 消 息 是 ROS 的 一 种 数据 类 
型 ,用 于 订阅 或 发 布 到 一 个 主题 。 主 题 是 用 于 标识 消息 内 容 的 名 称 。 节 点 通过 将 消息 发 布 
到 给 定 主题 来 发 出 消息 。 对 某 种 数据 感 兴趣 的 节点 将 订阅 适当 的 主题 。 对 于 单个 主题 可 以 
存在 多 个 并 发 发 布 者 和 订阅 者 ,单个 节点 也 可 以 发 布 和 订阅 多 个 主题 。 

1. 获取 主题 列表 


rostopic list 


说 明 : 运行 该 命令 能 够 获取 当前 活跃 的 主题 。 
例如 ,运行 海龟 仿真 的 例子 能 够 得 到 活跃 的 主题 如 下 : 


/rosout 

/rosout agg 
/turtlel/cmd vel 
/turtlel/color sensor 
/turtlel/pose 
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2. 打印 消息 内 容 


rostopic echo topic — name 


说 明 : 运行 该 指令 某 个 主题 上 发 布 的 消息 。 
例如 ,输入 命令 : 


rostopic echo rosout 


stamp: 
secs: 1481356969 
nsecs: 196580539 
frame id: '' 
level: 2 
name: /turtlesim 
msg: Spawning turtle [turtlel] at x= [5.544445], y= [5.544445], theta = [0.000000] 
file: /tmp/binarydeb/ros — indigo- turtlesim - 0.5.5/src/turtle frame.cpp 
function: string turtlesim: :TurtleFrame: :spawnTurtle 
line: 182 
topics: ['/rosout', '/turtlel/pose', '/turtlel/color sensor'] 


3. 测量 发 布 频率 
输出 每 秒 发 布 的 消息 数量 : 


rostopic hz topic - name 


说 明 : 运行 该 指令 能 够 获取 每 秒 发 布 的 消息 数量 。 
输出 每 秒 发 布 所 占 的 字 节 量 : 


rostopic bw topic - name 


说 明 : 运行 该 指令 能 够 获取 每 秒 发 布 消息 所 占 的 字 节 量 。 
4. 查看 主题 信息 


rostopic info topic - name 


说 明 : 能 够 获取 消息 类 型 .发布 者 和 订阅 者 。 
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5. 查看 消息 类 型 


rosmsg show message - type - nane 


说 明 : 运行 该 指令 能 够 查看 某 种 消息 类 型 的 信息 。 
6. 发 布 消息 


rostopic pub - r rate- in- hz topic - name message - type message - content 


说 明 :“rate” 为 指定 的 发 布 频率 ,用 一 个 数值 表示 ,后 面 的 三 项 分 别 是 主题 名 称 、 消 息 类 
型 和 信息 内 容 。 运 行 该 指令 能 够 手动 地 发 布 消 息 , 重 复 地 用 指定 的 频率 给 指定 的 主题 发 布 
指定 的 消息 。 

例如 ,前面 通过 键盘 控制 海 色 的 速度 和 方向 ,通过 发 布 消息 的 方法 也 可 控制 海龟 移动 。 
运行 下 面 指令 : 


rostopic pub -r 1 /turtlel/cmd vel geometry msgs/Twist '[1,0,0]' '[0,0,0]" 


上 面 的 指令 中 ,发 布 频率 为 1 ,发 布 内 容 [1,0,0] 表 示 线 速度 ,[0,0,0] 表 示 角 速度 。 仿 
真 效 果 为 海 色 沿 着 x 轴 方 向 一 直 前 进 。 


6.2.4 其 他 ROS 的 相关 概念 


(1) 元 包 (Stacks): Metapackages 是 专门 的 包 , 只 用 于 表示 一 组 相关 的 其 他 包 。 

(2) 参数 服务 器 (Parameter Server) : 允许 数据 通过 键 存储 在 中 央 位 置 。 它 目前 是 节点 
管理 器 的 一 部 分 。 

(3) 服务 (Services) : 发 布 /订阅 模型 是 一 种 非常 灵活 的 通信 范例 ,但 是 它 的 多 对 多 , 单 
向 传输 不 适合 请 求 /回复 交互 ,这 在 分 布 式 系统 中 通常 是 需要 的 。 请 求 /应 答 通过 服务 完成 ， 
服务 由 一 对 消息 结构 定义 : 一 个 用 于 请 求 , 另 一 个 用 于 应 答 。 

COD 包 (Bags) ; 包 是 用 于 保存 和 播放 ROS 消息 数据 的 格式 。 包 是 存储 诸如 传感器 数 
据 的 数据 的 重要 机 制 ,其 可 能 难以 收集 ,但 是 对 于 开发 和 测试 算法 是 必要 的 。 

(5) 分 布 (Distributions): ROS 分 布 区 是 可 以 安装 的 版 本 化 堆栈 的 集合 。 分 布 区 对 
Linux 发 行 版 起 着 类 似 的 作用 : 它们 使 得 安装 软件 集合 更 容易 ,并 且 它 们 还 在 一 组 软件 中 
保持 一 致 的 版 本 。 

(6) 存储 库 (Repositories) : ROS 依赖 于 代码 存储 库 的 联合 网 络 , 其 中 不 同 的 机 构 可 以 
开发 和 发 布 自己 的 机 器 人 软件 组 件 。 

(7) ROS wiki; ROS 社区 Wiki 是 记录 有 关 ROS 的 信息 的 主要 论坛 。 任 何人 都 可 以 注 
册 一 个 账户 ,并 提供 自己 的 文档 ,提供 更 正 或 更 新 , 写 教程 等 等 。 
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6.2.5 ”ROS 的 一 些 常用 工具 


1. rostopic 

rostopic 是 一 个 命令 行 工具 ,用 于 显示 有 关 ROS 主题 的 调试 信息 ,包括 发 布 者 .订阅 
者 \ 发 布 速率 和 ROS 消息 。 

选项 ， 

(1) rostopic echo [topic name]: 显示 发 布 到 主题 的 消息 。 

(2) rostopic list -v; 详细 显示 当前 主题 的 列表 。 

(3) rostopic hz [topic name]: 显示 主题 的 发 布 速率 。 默 认 情 况 下 报告 的 速率 是 
rostopic 已 经 运行 的 整个 时 间 内 的 平均 速率 。 

(4) rostopic pub [topic name] [msg type] [data]: 将 数据 发 布 到 主题 。 

2. rqt_console(a) 

它 提供 了 一 个 用 于 显示 和 过 滤 ROS 消息 的 GUI 插件 。 

3. rqt logger level(b) 

它 提供 了 一 个 用 于 配置 ROS 节点 的 记录 器 级 别 的 GUI 插件 。 

4. rqt graph 

它 提供 了 一 个 用 于 可 视 化 ROS 计算 图 的 GUI 插件 。 


6.3 ROS 的 程序 包 的 创建 与 编译 


6.3.1 创建 工作 区 和 功能 包 


1. 创建 工作 区 

ROS 的 各 类 功能 包 都 存放 在 工作 区 中 ,通过 编译 工作 区 来 编写 包含 在 它 目录 下 的 所 有 
功能 程序 。 同 一 网 络 中 的 功能 包 应 该 放 在 同一 个 工作 区 ,以 方便 调试 和 交互 。 对 于 不 相同 
或 不 相关 的 程序 ,应 该 避免 放 在 同一 个 工作 区 中 。 这 样 可 以 区 分 不 同 的 大 型 程序 ,也 可 以 节 


省 编译 时 间 , 以 提高 编译 效率 。 
对 于 ROS 程序 的 编写 步骤 ,首先 是 创建 一 个 能 够 容纳 功能 包 的 工作 区 ,然后 在 这 个 工 
作 区 中 创建 相应 的 功能 包 。 


COD 用 一 个 mkdir 命令 创建 一 个 新 的 文件 夹 作 为 工作 区 ,例如 创建 的 文件 夹 在 
— / home/yang/ros 中 ,运行 下 面 的 命令 : 


cd 一 
mkdir -p yang/ros 


(2) 在 目录 创建 新 的 文件 夹 后 ,需要 将 此 新 路 径 添加 到 ROS PACKAGE. PATH. R 
需要 在 一 /. bashrc 文件 的 末尾 添加 一 个 新 行 : 
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echo "export ROS PACKAGE PATH = 一 /Yang/ros: $ (ROS PACKAGE PRTH] ">> 一 /.bashrc 
一 /.bashrc 


运行 以 上 命令 后 ,将 完成 在 ROS 中 创建 和 配置 新 的 工作 区 。 
(3) 查看 ROS 正在 使 用 的 工作 空间 ,命令 如 下 : 


$ echo $ ROS PACKAGE PATH 


(4) 初始 化 工作 区 ,命令 如 下 : 


cd 一 /Yang/ros 
catkin init workspace 


(5) 编译 工作 区 ,命令 如 下 : 


cd 一 /yang 
catkin make 


2. 创建 功能 包 
在 新 创建 的 工作 区 中 创建 一 个 ROS 功能 包 : 


catkin create pkg examplel 


如 图 6-8 所 示 ,为 功能 包 成 功 创建 时 的 界面 。 此 时 查看 工作 区 发 现 多 了 两 个 文件 : 
package. xml 和 CMakeLists, txt, 都 是 与 功能 包 相 关 的 配置 文件 。 其 中 ,package. xml 是 清 
单 文件 ,前 文 已 指出 ,清单 文件 定义 了 一 个 程序 包 , 包 括 了 包 的 名 称 、 版 本 和 依赖 关系 等 内 
容 。CMakeLists. txt 是 一 个 Cmake 脚本 文件 ,包括 了 各 种 编译 指令 ,例如 与 生成 的 可 执行 
文件 类 型 源 文件 、 头 文件 和 链接 库 等 相关 的 指令 


图 6-8 功能 包 创建 成 功 界面 


注意 : CMake 是 一 个 跨 平 台 的 编译 工具 ,可 以 用 简单 的 语句 来 描述 所 有 平台 的 编译 过 程 。 
6.3.2 ROS 程序 的 编译 过 程 
如 图 6-9 所 示 ,使 用 ROS 的 catkin 编译 系统 进行 程序 的 编译 有 4 个 步骤 。 


声明 可 执行 ux z 执行 setup.bash 
= 文件 “| 编译 工作 区 m. 各 时 


图 6-9 编译 系统 的 4 个 步骤 


声明 依赖 库 
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1. 依赖 关系 及 依赖 库 的 声明 

1) 一 级 依赖 

之 前 在 使 用 catkin_create_pkg 命令 时 提供 了 几 个 程序 包 作为 依赖 包 , 现 在 可 以 使 用 
rospack 命令 工具 来 查看 一 级 依赖 包 。 


$ rospack dependsl beginner tutorials 
. std_msgs 

* — rospy 

. roscpp 


rospack 列 出 了 在 运行 catkin create pkg 命令 时 作为 参数 的 依赖 包 , 这 些 依赖 包 随 后 
保存 在 package. xml 文件 中 。 


$ roscd beginner tutorials 


$ cat package. xml 
* < package > 


<buildtool depend> catkin </buildtool_depend > 
X build depend» roscpp </build depend> 

X build depend» rospy </build depend> 

X build depend» std msgs «/build depend» 


«/package » 


2) 间接 依赖 
在 很 多 情况 下 ,一 个 依赖 包 还 会 有 它 自 己 的 依赖 包 ,例如 ,rospy 还 有 其 他 依赖 包 。 


$ rospack dependsl rospy 
* genpy 
rosgraph 
à rosgraph msgs 
. roslib 
. std msgs 


一 个 程序 包 还 可 以 有 好 几 个 间接 的 依赖 包 , 使 用 rospack 可 以 递归 检测 出 所 有 的 依 
赖 包 。 


$ rospack depends beginner tutorials 
Cpp common 

rostime 

roscpp traits 

roscpp serialization 
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genmsg 

genpy 

message runtime 
rosconsole 
std msgs 
rosgraph msgs 
xmlrpcpp 
roscpp 
rosgraph 
catkin 
rospack 
roslib 

rospy 


因此 ,对 于 一 个 ROS 程序 ,需要 声明 程序 所 依赖 的 功能 包 , 以 让 catkin 定位 编译 程序 包 
所 需要 的 头 文件 和 链接 库 。 在 程序 包 目 录 下 相应 的 CMakeLists. txt 文件 有 以 下 的 命令 行 : 


find package(catkin REQUIRED) 
所 依赖 的 其 他 包 需 添加 到 这 一 行 的 关键 字 COMPONENTS 后 面 ,如 下 所 示 : 
find package(catkin REQUIRED COMPONENTS package - names) 
此 外 ,还 需 在 package. xml 文件 中 列 出 依赖 库 , 通 过 编译 依赖 和 运行 依赖 两 个 关键 字 实 现 : 


X build depend package - name «/build depend > 
< run depend» package - nane «/run depend» 


2. 声明 可 执行 文件 

对 于 程序 包 中 的 每 一 个 可 执行 文件 , 需 在 CMakeLists. txt 文件 中 添加 两 句 命令 ,声明 
需要 创建 的 可 执行 文件 。 第 一 行 声明 可 执行 文件 的 文件 名 ,后 面 跟着 相应 的 源 文 件 列表 。 
其 中 若 包含 多 个 源 文件 , 则 用 空格 分 隔 开 : 


add executable(executable - name source - filel source -file2 … source- fileN) 
第 二 行 声 明 所 需要 的 链接 库 : 
target link libraries(executable - name $ {catkin LIBRARIES}) 


3. 编译 工作 区 
运行 以 下 的 命令 可 以 编译 包 中 的 所 有 可 执行 文件 : 


catkin make 
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4. 执行 setup. bash 脚本 
运行 以 下 命令 可 以 执行 名 为 setup. bash 的 脚本 文件 : 


source devel/setup. bash 


5. Hello World 程序 的 编译 与 执行 

与 其 他 基于 计算 机 语言 的 程序 一 样 ,ROS 的 程序 也 需要 进行 代码 编写 ,代码 编译 与 程 
序 执行 ,才能 够 完成 一 定 的 功能 。 

对 于 一 个 简单 的 "Hello World” 程 序 , 它 的 代码 如 下 : 


//ThisisaROSversionofthestandard"Hello, world" 
// 这 是 一 个 ROS 版 本 的 标准 Hello World 程序 
//This header definesthe standard ROS classes. 
//( 这 是 ROS 类 头 文件 的 声明 ) 
井 include <ros /ros.h> 

int main (int argc, char * * argv)( 
//1nitialize the ROS system. 
// 对 ROS 系统 进行 初始 化 

ros::init (argc, argv , " hello ros"); 
//Establ ish this program as a ROS node. 
// 将 这 个 程序 建立 成 ROS 的 一 个 节点 
ros: : NodeHandle nh; 
//Send some output as a log message. 
// 发 送 一 些 日 志 消息 
ROS INFO STREAM(" Hello ,world! "); 
) 


编写 完 上 面 的 代码 后 ,将 文件 保存 为 HelloWorld. cpp 的 名 称 。 对 上 面 的 代码 说 明 
如 下 : 

CD. 函数 ros::init() 用 于 初始 化 ROS 客户 端 库 , 需 要 在 程序 的 起 始 处 调用 一 次 这 个 函 
数 。ros::init() 函 数 最 后 的 参数 是 一 个 包含 节点 默认 名 的 字符 串 。 

(2) 函数 ros::NodeHandle( 节 点 句柄 ) 对 象 是 程序 用 于 和 ROS 系统 交互 的 主要 机 制 。 
创建 此 对 象 会 将 程序 注册 为 ROS 节点 管理 器 的 节点 。 最 简单 的 方法 就 是 在 整个 程序 中 只 
创建 一 个 NodeHandle 对 象 。ROS_INFO_STREAM 宏 将 生成 一 条 消息 , 且 这 一 消息 被 发 
送 到 不 同 的 位 置 , 包 括 控制 台 窗口 。 

完成 代码 的 编写 后 ,根据 编译 原理 对 CMakeLists. txt 文件 进行 编辑 ,声明 所 需要 的 依 
赖 库 和 可 执行 文件 ,并 编译 工作 区 。 


t # What version of CMake is needed ?( 说 明 所 需要 CMake 的 最 小 版 本 ) 
2 cmake minimum required (VERSION 2.8.3) 
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3# Name of this package. 

4 project (agitr) 

5 # Find the catkin build system, and any other packages on 

6 # which we depend. 

7 find pa cka ge (catkin REQUIRED COMPONENTS roscpp) 

8# Declare our catkin package. 

9catkin package() 

104 Specify locations of header files. 

llinclude directories(include $ (catkin INCLUDE DIRS ]) 

12 & Declare the executable, along with its sourcefiles. If there are multiple 
13 # executables, use multiple copies of this line. 

14 add executable(hello hello.cpp) 

15: Specify libraries against which to link. Again, this line should be copied for each 
16# distinct executable in the package. 

17 target link libraries (hello $ (catkin LIBRARIES ]) 


当 所 有 这 些 编译 步骤 完成 后 ,要 执行 这 个 程序 时 ,首先 要 启动 roscore: 一 个 程序 就 是 
个 节点 ,而 一 个 节点 需要 一 个 节点 管理 器 才 可 以 正常 运行 。 然 后 可 使 用 指令 rosrun 来 执 
行 ,输入 以 下 命令 : 


rosrun agitrhello 
这 个 程序 会 在 终端 打印 类 似 于 下 面 这 样 的 输出 : 


[INFO] [1416432122.659693753]: Hello ,world! 


6.4 ROS 5 MATLAB & 


第 1 章 介 绍 了 MATLAB 拥有 机 器 人 原型 设计 和 机 器 人 仿真 控制 系统 等 强大 的 工具 。 
然而 ,MATLAB 与 ROS 之 间 的 集成 还 存在 着 一 定 的 难度 。 目 前 ,MATLAB 与 ROS 之 间 
集成 已 经 有 多 种 解决 方案 ,包括 基于 JavaScript 对 象 符号 的 rosbridge( 见 http; //wiki, ros. 
org/rosbridge suite) ,基于 Java 的 ROS-MATLAB bridge 和 ROSlab-IPCbridge 等 。 然 而 ， 
这 些 集成 都 没有 广泛 应 用 ,因为 它们 的 安装 比较 困难 ,或 者 是 在 可 用 性 和 实用 性 上 存在 着 一 
定 的 缺陷 。 

第 1 章 介 绍 了 Robotics System Toolbox(RST) 工 具 箱 具有 的 几 个 主要 功能 : 路 径 
规划 、 路 径 跟 踪 和 地 图 表示 算法 ,用 于 在 不 同 旋转 和 平移 表示 之 间 转 换 的 函数 ,与 启用 
ROS 的 机 器 人 进行 双向 通信 。 随 着 MATLAB 2015a 的 发 布 , MATLAB 与 ROS 之 间 集 成 
有 一 个 更 好 的 选择 。 新 的 MATLAB 版 本 引入 新 的 机 器 人 系统 工具 箱 (Robotics System 
Toolbox) 。 
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6.4.1 RST 的 ROS 功能 介绍 


下 面 介绍 RST 为 MATLAB 与 ROS 之 间 提 供 通 信 的 功能 。RST 除了 提供 MATLAB 
和 Simulink 之 间 的 接口 ,还 提供 了 MATLAB 与 ROS 之 间 的 接口 ,使 得 MATLAB 能 够 与 
ROS 网 络 通信 。 

MATLAB 与 ROS 的 接口 的 功能 有 多 种 。 首 先是 让 用 户 能 够 进行 交互 式 地 探索 机 器 
人 功能 和 可 视 化 传感器 数据 。 其 次 ,用 户 可 以 启用 可 应 用 于 ROS 操作 系统 的 机 器 人 仿真 器 
(如 Gazebo 和 V-REPO ,利用 它们 去 设计 开发 机 器 人 模型 ,并 将 仿真 实验 数据 导 和 人 
MATLAB, 借 助 MATLAB 强大 的 工具 箱 及 数学 工具 去 测试 和 验证 用 户 的 机 器 人 算法 和 应 
用 程序 的 性 能 。 最 后 ,用 户 还 可 以 直接 在 MATLAB 和 Simulink 中 创建 自 包 含 ROS 的 网 
络 , 并 导入 ROS 日 志文 件 (rosbags)。 借 助 MATLAB/Simulink 对 离线 数据 进行 可 视 化 ,分 
析 和 后 处 理 。 这 些 功能 允许 用 户 在 MATLAB 和 Simulink 中 开发 机 器 人 算法 ,同时 使 用 户 
能 够 与 ROS 网 络 上 的 其 他 节点 交换 消息 。 此 外 ,通过 使 用 EmbeddedCoder, 用 户 可 以 从 
Simulink 模型 生成 C++ 代码 ,因此 MATLAB 中 编写 的 算法 也 可 以 在 安装 了 ROS 的 任何 
Linux 平 台 上 运行 独立 的 ROS 应 用 程序 。 

总 之 ,RST 的 主要 功能 允许 用 户 : 

CD 与 ROS 网 络 通信 ,交互 式 探索 机 器 人 功能 ,并 可 视 化 传感器 数据 ; 

(2) 直接 从 MATLAB 和 Simulink 创建 ROS 节点 ,发 布 者 和 订阅 者 ; 

(3) M MATLAB 和 Simulink 创建 和 发 送 ROS 消息 ; 

(D M MATLAB 和 Simulink 创建 和 发 送 ROS 自 定义 消息 ; 

(5) 调用 并 提供 ROS 服务 ; 

(6) 导入 ROS 日 志文 件 以 可 视 化 、 分 析 和 后 处 理 记录 的 数据 ; 

CD 在 任何 (包括 Windows.Linux. Mac) 平 台 上 使 用 ROS 功能 ; 

(8) 使 用 MATLAB 作为 ROS 主机 ; 

(9) 在 启用 ROS 的 机 器 人 和 机 器 人 仿真 器 (如 Gazebo 和 V-REP) 上 测试 和 验证 应 用 


(10) 创建 与 ROS 网 络 一 起 使 用 的 Simulink 模型 ; 
(11) 从 Simulink 模型 生成 独立 的 ROS C++ 节点 。 


6.4.2 MATLAB 与 ROS 通信 的 介绍 

1. 通信 

简单 介绍 MATLAB 与 ROS 进行 通信 的 功能 如 下 。 

首先 在 安装 有 ROS 的 主机 运行 ROS 系统 ,然后 我 们 用 ROS 主机 的 IP 地 址 和 端口 号 
来 初始 化 MATLAB ROS 子 系统 ,运行 下 面 的 指令 : 


rosinit('192.168.1. 10',11311). 
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如 果 没 有 提供 参数 ,那么 MATLAB 将 创建 一 个 ROS 主 服务 器 ,然后 会 显示 它 的 URI, 
以 便于 被 其 他 节点 使 用 。 

2. 发 布 主题 

接 下 来 要 发 布 一 个 主题 。 在 这 里 使 用 一 个 简单 的 例子 进行 说 明 。 首 先 我 们 将 创建 一 个 
标准 字符 串 类 型 的 消息 对 象 ,然后 设置 它 的 值 , 输 入 以 下 指令 ; 


msg = rosnessage('std nsgs/String'); 
msg.Data - 'hello, MATLAB'; 


消息 是 一 个 对 象 , 它 的 属性 是 分 层 的 ,并 且 匹 配 消息 的 字段 。 可 以 直接 读 取 或 写 人 属 
性 ,不必 使 用 setter 或 getter 方法 。 
然后 进行 发 布 ,输入 以 下 指令 : 


rospublisher( '/MyTopic', msg) ; 
或 者 ,可 以 创建 一 个 发 布 者 对 象 并 可 选择 指定 消息 类 型 ,输入 以 下 指令 : 
pub = rospublisher('/MyTopic', 'std msgs/String'); 
然后 调用 它 的 send 方法 .输入 以 下 指令 : 
pub. send( msg) ; 


可 以 在 构建 时 为 发 布 方 对 象 配置 各 种 选项 。 

3. 接收 主题 

进行 接收 主题 已 比较 简单 。 首 先 ,为 特定 主题 创建 一 个 订阅 者 对 象 ,并 可 选择 指定 消息 
类 型 ,输入 以 下 指令 : 


sub = rossubscriber('/MyTopic', 'std_msgs/String') 


构造 函数 有 各 种 选项 来 控制 缓冲 区 大 小 ,以 及 是 否 只 返回 最 近 的 消息 。 阅 读 关 于 主题 
的 下 一 条 消息 ,输入 以 下 指令 : 


msg = sub. receive() 


这 时 , 它 将 会 处 于 阻塞 状态 ,直到 接收 到 消息 才 解 除 该 状态 。 但 是 我 们 也 可 以 指定 超时 间隔 
(以 秒 为 单位 ) 将 解除 该 状态 ,输入 以 下 指令 : 


msg = sub.receive(5) 


对 轮 询 消息 的 蔡 代 方法 是 建立 回调 ,输入 以 下 指令 : 
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sub = rossubscriber('/MyTopic', std msgs/String', @rxcallback) 


对 于 函数 : 


function rxcallback (src, msg) disp([char(msg. Data ( )), sprintf('\nMessage received: %s', 
datestr(now))]); 


它 在 每 个 消息 接收 上 被 调用 。 
接 下 来 ,看 一 个 更 复杂 的 消息 : 


msg = rosmessage('geometry msgs/TwistStamped'), 
我 们 可 以 看 到 它 的 定义 ,输入 以 下 指令 : 
>> definition(msg) 
或 访问 其 中 一 个 字段 输入 以 下 指令 : 
msg. Twist.Linear.X = 0; 
ROS 参数 可 以 通过 返回 的 Parameter Tree 对 象 进行 访问 ,输入 以 下 指令 : 
ptree = rosparam, 
此 外 ,可 以 使 用 它 来 设置 ,获取 ,创建 或 删除 ROS 参数 服务 器 中 的 参数 ,输入 以 下 指令 : 


ptree. get( 'rosversion') 
ptree. set( 'myparameter', 23) 


4. 访问 和 创建 服务 

同时 ,参数 可 以 具有 整数 .逻辑 .字符 、 双 精度 或 单元 格 数组 类 型 ,还 可 以 在 MATLAB 
代码 中 访问 和 创建 服务 。 在 这 里 以 一 个 简单 的 例子 进行 介绍 ,可 以 创建 一 个 服务 来 添加 两 
个 整数 。 输 入 以 下 指令 : 


sumserver = rossvcserver('/sum', rostype.roscpp tutorials TwoInts, @SumCallback), 
实行 一 定 的 服务 功能 ,输入 以 上 指令 : 
function resp = SumCallback(—,req,resp)resp.Sum = req.A *req.B; 


现在 ,我 们 可 以 从 任何 ROS 节点 进行 调用 .输入 以 下 指令 : 
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$ rosservice call /sum2 12 sum: 3 


或 者 ,首先 从 MATLAB 内 部 创建 一 个 服务 客户 端 ,输入 以 下 指令 : 


sumclient = rossvcclient( '/sum'), 


然后 ,创建 带 有 要 添加 的 号 码 的 消息 ,输入 以 下 指令 : 


sumreq = rosmessage(sumclient); 
sumreq. A 2; 
sumreq.B = 1, 


最 后 输入 以 下 指令 ,调用 该 服务 : 


sumresp = call(sumclient, sumreq, 'Timeout',3) 
>> sumresp.Sum ans = 3. 


5. 读 写 ROS 包 文件 
首先 ,打开 bag 文件 并 列 出 可 用 的 主题 ,输入 以 下 指令 : 


bag = rosbag ('quad- 2014 - 06 - 13. bag')bag. AvailableTopics. 


要 提取 主题 /预览 上 的 所 有 图 像 ,使 用 select 方法 选择 特定 主题 ,使 用 readMessages 提 
取 100 个 消息 的 单元 格 数组 ,其 类 型 为 sensormsgs/Image, 然 后 将 其 转换 为 单元 格 数组 的 
可 以 显示 的 图 像 。 输 入 以 下 指令 : 


preview = bag.select('Topic', 'preview') 
subset = preview. readMessages(500:599) 
images = cellfun((Zreadlmage,all, 'UniformOutput', false); 


我 们 还 可 以 提取 惯性 测量 单元 (IMU) 数 据 , 并 且 将 zx 轴 平 移 和 < 轴 角 速度 放 管 到 一 个 
时 间 序 列 (Timeseries) 对 象 中 。 它 将 会 自动 拾取 消息 的 时 间 , 并 具有 用 于 分 析 和 显示 的 各 
种 方法 ,输入 以 下 指令 : 


imu = bag.select('Topic', 'fcu/imu'); 
ts = imu.timeseries( 'LinearAcceleration. X', 'AngularVelocity.Z'); 
ts. plot 


6. MATLAB 中 的 交互 式 命令 
在 MATLAB 环境 中 有 许多 交互 式 命令 ,它们 模仿 ROS 命令 行 , 来 查找 消息 、 节 点 、 主 
Bi .参数 或 服务 ,例如 ,以 下 指令 : 
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>> rosmsg list 

>> rosmsg show geometry msgs/TwistStamped 
>> rosnode list 

>> rostopic list 

>> rosparam list 

>> rosservice list 


它们 提供 了 在 MATLAB 中 编写 代码 所 需 的 所 有 编程 工具 ,可 以 完全 参与 基于 ROS 的 机 器 
人 控制 系统 。MATLAB-ROS 的 一 个 强大 的 优势 是 它 的 平台 独立 性 。 它 的 代码 可 以 在 
Mac、Windows 或 Linux 系统 上 工作 。 

7. ROS 与 Simulink 

程序 化 实现 的 另 一 种 方法 是 使 用 Simulink 框图 为 环境 建 模 。RST 提供 了 一 个 包含 
Publish 和 Subscribe 的 板块 。 例 如 ,Subscribe 板块 的 Msg 输出 是 总 线 类 型 ,我 们 可 以 使 用 
Simulink 总 线 选 择 器 来 提取 特定 消息 字段 。 使 用 触发 子 系统 来 确保 仅 在 接收 到 消息 之 后 
才 发 布 消息 。 通 过 适当 的 Simulink 设置 ,此 控制 器 可 以 * 实 时 ?运行 ,Scope 块 允许 我 们 方便 
地 查看 发 生 了 什么 。 当 然 ,我 们 可 以 将 信号 记录 到 工作 空间 变量 ,以 供 以 后 分 析 和 图 形 
显示 。 

最 后 ,可 以 将 生成 的 图 用 代码 导出 。 这 时 ,Simulink 将 生成 一 个 格式 为 . tgz 的 文件 。 
它 包含 在 Linux 系统 上 构建 独立 实时 ROS 节点 所 需 的 所 有 代码 。 

本 节 介 绍 了 MATLAB *j ROS 之 间 通 信 的 基本 知识 。 随 着 需求 加 大 和 技术 发 展 ,我 们 
相信 ,MATLAB 的 RST 工具 箱 将 开发 出 更 丰富 更 强大 的 功能 ,使 得 科研 人 员 和 工程 师 能 
够 更 方便 、 更 有 效 地 创建 机 器 人 控制 系统 ,并 利用 大 型 和 成 熟 的 ROS 代码 库 。 关 于 RST 实 
现 ROS 与 MATLAB/Simulink 之 间 的 更 多 例子 ,可 以 参照 网 站 : 


https://www. mathworks. com/hardware-support/robot-operating-system. html 


6.5 ROS 5 V-REP 之 间 的 集成 


在 V-REP 部 分 中 提 到 ,在 ROS 的 工作 机 制 中 ,V-REP 可 以 作为 一 个 ROS 节点 ,能 够 
通过 3 种 方式 与 其 他 ROS 节点 进行 通信 : V-REP 节点 提供 ROS 服务 ; V-REP 节点 作为 
ROS 发 布 者 ; V-REP 节点 作为 ROS 用 户 。 其 中 ,有 4 种 插件 用 于 V-REP 与 ROS 之 间 的 
通信 : RosInterface, RosPlugin, ROS skeleton 插件 和 ROS V-Rep Bridge。 其 中 ,RosInterface 和 
RosPlugin 是 V-REP 官方 开发 的 插件 。 


6.5.1 V-REP 中 的 ROS 程序 包 


V-REP 中 的 一 般 ROS 功能 通过 两 个 不 同 的 接口 支持 : RosInterface (libv _ 
repExtRosInterface. so) 和 RosPlugin(libv repExtRos. so)。 其 中 ,RoslInterface 的 使 用 更 
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加 方便 简单 ,与 ROS 的 C/C++ API 相似 ,推荐 使 用 。 

V-REP 的 Linux 发 行 版 应 该 已 经 在 V-REP/compiledRosPlugins 包括 这 些 已 经 编译 好 
的 文件 ,但 是 它们 首先 需要 复制 到 V-REP 的 主 文件 夹 中 ,否则 这 些 插 件 在 V-REP 运行 时 
不 会 被 加 载 。 然 而 , 当 一 切 准备 就 绪 后 ,也 可 能 会 遇 到 插件 加 载 的 问题 ,具体 取决 于 系统 的 
特定 性 。 当 V-REP 启动 时 加 载 插件 ,确保 始终 检查 V-REP 的 终端 窗口 有 关 搬 件 加 载 操作 
的 详细 信息 。 如 果 roscore 当时 运行 , 则 ROS 插件 将 会 成 功 加 载 并 进行 初始 化 。 

如 果 无 法 加 载 插件 ,那么 应 该 自己 重新 编译 它们 。 它 们 是 开源 的 ,可 以 根据 需要 进行 修 
改 , 以 支持 特定 功能 或 扩展 其 功能 。programming/ros_packages 文件 夹 程序 包含 以 下 8 个 
软件 程序 包 : 

(1) ros. bubble rob; 这 是 一 个 机 器 人 控制 器 的 程序 包 , 通 过 RosPlugin 连接 到 
V-REP。 在 下 面 的 例子 中 ,该 节点 将 负责 控制 演示 场景 controlTypeExamples. ttt 中 的 暗 红 
色 机 器 人 。 

(2) ros bubble rob2; 这 是 一 个 机 器 人 控制 器 的 程序 包 , 通 过 RosInterface 连接 到 V-REP。 
在 下 面 的 例子 中 ,该 节点 将 负责 控制 示例 场景 control TypeExamples. ttt 中 的 亮 红色 机 
器 人 。 

(3) vrep common: 此 程序 包 用 于 生成 实现 RosPlugin 的 V-REP API 函数 所 需 的 服务 
和 流 消息 。 使 服务 和 流 消息 在 单独 的 程序 包 中 允许 其 他 应 用 使 用 它们 ,以 便 以 方便 的 方式 
经 由 RosPlugin 与 V-REP 通信 。 

(4) v_repExtRosInterface: 这 个 程序 包 是 RosInterface, 将 被 编译 为 *. so” 文 件 ,并 由 
V-REP 使 用 。 

(5) vrep_ plugin: 这 个 程序 包 是 将 被 编译 为 “. so” 文 件 ,并 由 V-REP 使 用 的 
RosPlugin。 

(6) vrep joy: 此 软件 程序 包 支 持 与 操纵 杆 进行 交互 。 

(7) vrep skeleton msg and srv; 这 个 程序 包 等 效 于 vrep_common, 并 且 可 以 用 于 为 
V-REP 创建 特定 的 ROS 插件。 例如 ,为 特定 机 器 人 支持 ROS 消息 。 

(8) vrep_plugin_skeleton: 这 个 程序 包 等 效 于 vrep_plugin, 也 和 上 面 的 软件 程序 包 相 
似 , 可 以 用 于 为 V-REP 创建 自己 的 特定 ROS 插件 。 


6.5.2 在 ROS 中 安装 V-REP 


在 ROS 中 安装 V-REP 的 具体 步骤 是 : 
(D 在 V-REP 的 官网 下 载 相 应 版 本 的 V-REP; 
© 在 ROS 的 终端 中 通过 以 下 指令 转 到 V-REP 所 在 的 文件 夹 并 要 运行 它 : 


~./vrep. sh; 


© 把 programming/ros packages 文件 夹 中 的 程序 包 复 制 到 catkin_ws/src 文件 夹 中 ， 
确保 ROS 知道 这 些 程序 包 , 即 可 以 切换 到 以 上 程序 包 文件 夹 。 可 以 用 以 下 命令 : 
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roscd vrep ros interface 

roscd vrep plugin 

roscd vrep common 

roscd vrep joy 

roscd ros bubble rob 

roscd ros bubble rob2 

roscd vrep skeleton msg and srv 


A 40 4p XD XD 40 40 4 


roscd vrep plugin skeleton 


6.5.3 在 ROS 中 创建 相关 的 V-REP 程序 包 
CD 为 了 构建 软件 程序 包 ,切换 到 catkin_ws 文件 夹 并 键入 ， 


$ export VREP_ROOT = ~/path/to/v_rep/folder 
$ catkin build 


(2) 软件 程序 包 应 该 已 经 生成 并 编译 为 可 执行 文件 或 库 。 将 创建 的 文件 复制 并 粘贴 到 


V-REP 安装 文件 夹 中 ,插件 即 可 以 使 用 了 。 确 保 已 安装 所 有 必需 的 项 目 , 使 编译 工作 没有 
问题 。 例 如 ,vrep_joy 程序 包 需 要 安装 joystick 驱动 程序 为 : 


$ sudo apt- get install ros- indigo - pr2 - controllers ros - indigo- joystick - drivers 


(3) V-REP 中 的 两 个 可 用 ROS 插件 ( 即 RosInterface 和 RosPlugin) 可 以 并 行 操作 ,但 
是 在 这 里 我 们 将 使 用 RosInterface. 因为 它 更 灵活 ,并 且 还 支持 大 多 数 标准 ROS 消息 。 接 
下 来 ,我 们 将 继续 介绍 基于 RosInterface 的 方法 。 一 旦 有 了 RosInterface 库 ,打开 一 个 终端 
并 启动 : 


ROS master: $ roscore 
(4) 打开 另 一 个 终端 ,移动 到 V-REP 安装 文件 夹 并 启动 V-REP。 运 行 下 面 的 指令 : 


$ ./vrep. sh 

License file 'v rep': 

cS 

Simulator launched. 

Plugin 'BubbleRob': loading... 
Plugin 'BubbleRob': load succeeded. 
Plugin 'K3': loading... 

Plugin 'K3': load succeeded. 
Plugin 'RemoteApi': loading... 
Plugin 'RemoteApi': load succeeded. 
Plugin 'RosInterface': loading... 
Plugin 'RosInterface': load succeeded. 
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(5) 成 功 地 加 载 Ros Interface 后 ,检查 可 用 的 节点 ,运行 下 列 指令 : 


$ rosnode list 
/rosout 
/vrep ros interface 


(6) 在 空 的 V-REP 场景 中 ,选择 一 个 对 象 ,然后 依次 选择 菜单 栏 添 加、 关联 的 子 脚 本 、 
非 线程 ,将 非 线程 子 脚本 附加 到 它 。 打 开 该 脚本 的 脚本 编辑 器 ,并 将 内 容 蔡 换 为 以 下 内 容 : 


function subscriber callback(msg) 
—- This is the subscriber callback function 
simAddStatusbarMessage('subscriber receiver following Float32: '..msg.data) 


end 


function getTransformStamped(objHandle, name, relTo, relToName) 
—- This function retrieves the stamped transform for a specific object 
t= sinGetSystenTine( ) 
p= simGetObjectPosition(objHandle, relTo) 
07 simGetObjectQuaternion(objHandle, relTo) 
return ( 
header = ( 
stamp = t, 
frame_id = relToName 


)h 

child frame id- name, 

transform= ( 
translation- (x7 p[1],y7 p[2],z  p[3]), 
rotation- (x- o[1],y * o[2],z - o[3],w - o[4]) 


end 


if (sim call type-- sim childscriptcall initialization) then 
—- The child script initialization 
objectHandle = simGetObjectAssociatedWithScript(sim handle self) 
objectName = simGetObjectName(objectHandle) 
—- Check if the required RosInterface is there: 
moduleName - 0 
index - 0 
rosInterfacePresent - false 
while moduleName do 
moduleName = simGetModuleName( index) 
if (moduleName == 'RosInterface') then 
rosInterfacePresent - true 
end 
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index- index+1 
end 


—- Prepare the float32 publisher and subscriber (we subscribe to the topic we advertise): 
if rosInterfacePresent then 
publisher = simExtRosInterface advertise('/simulationTime', std nsgs/Float32') 


subscriber = simExtRosInterface subscribe(' /simulationTime', ' std msgs/| Float32','subscriber | 
callback') 

end 
end 


if (sim call type-- sim childscriptcall actuation) then 
—-— Send an updated simulation time message, and send the transform of the object attached 
to this script: 
if rosInterfacePresent then 
simExtRosInterface publish(publisher, (data = simGetSimulationTime()]) 


simExtRosInterface sendTransforn(getTransformStamped(objectHandle, objectName, — 1, 'world')) 
—- To send several transforms at once, use simExtRosInterface sendTransforms instead 
end 
end 


if (sim call type-- sim childscriptcall cleanup) then 
-- Following not really needed in a simulation script (i.e. automatically shut down at 
simulation end): 
if rosInterfacePresent then 
simExtRosInterface shutdownPublisher(publisher) 
simExtRosInterface shutdownSubscriber(subscriber) 
end 
end 


(7) 以 上 嵌入 式 子 脚 本 将 发 布 仿真 的 时 间 ,并 同时 订阅 相对 的 主题 。 它 还 将 发 布 脚本 
附加 到 的 对 象 的 变换 中 。 输 入 以 下 指令 查看 仿真 时 间 主 题 ; 


$ rostopic list 
如 果 想 要 查看 消息 内 容 , 输 入 以 下 指令 : 


$ rostopic echo /simulationTime 


6.5.4 使 用 ROS 节点 控制 V-REP 模型 的 例子 


在 V-REP 中 ,加 载 演示 场景 “controlTypeExamples. ttt”, 并 运行 仿真 。 如 图 6-10 所 
示 ,该 场景 说 明了 V-REP 目前 支持 的 6 种 控制 方法 ,可 由 能 入 的 子 脚本 、 搬 件 . 远 程 API 客 
户 端 ,RosInterface,RosPlugin 和 定制 的 客户 程序 控制 。 其 中 ,这 6 种 方法 彼此 兼容 ,并且 也 
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可 以 组 合 ,而 且 所 有 方法 都 与 V-REP 分 布 式 控制 架构 兼容 。 本 部 分 重点 关注 通过 
RosInterface 控制 的 亮 红 色 机 器 人 。 


由 远程 API 由 Rosinterface 由 RosPlugin 
DUE NENNEN 控制 


由 定制 的 客户 端 
程序 控制 


图 6-10 V-REP 提供 的 演示 场景 


子 脚本 附加 到 亮 红 色 机 器 人 ,并 以 非 线 程 方式 运行 ,执行 以 下 功能 : 

CD) 确定 一 些 物体 手柄 (例如 ,马达 关节 手柄 和 接近 传感器 手柄 )， 

(2) 验证 是 否 加 载 了 RosInterface; 

(3) 添加 场景 中 相关 的 标语 信息 ; 

(4) 启动 与 电机 速度 相关 的 订阅 者 ; 

(5) 启动 与 传感器 发 布 者 和 仿真 时 间 有 关 发 布 者 ; 

(6) 最 终 启 动 一 个 客户 端 应 用 程序 。 调 用 应 用 程序 时 使 用 一 些 主 题名 称 作 为 参数 ,以 
便 它 知 道 要 监听 和 订阅 的 主题 。 然 后 运行 客户 端 应 用 程序 (rosBubbleRob2) ,通过 ROS 接 
管 亮 红色 机 器 人 的 控制 。 

现在 停止 仿真 并 打开 一 个 新 场景 ,然后 将 以 下 模型 拖 入 其 中 : Models/tools/ 
rosInterface helper tool. ttm。 此 模型 由 提供 以 下 主题 发 布 者 和 订阅 者 的 单个 自 定义 脚本 构成 : 

(1) startSimulation 主题 : 可 以 用 于 通过 发 布 此 主题 std_msgs:: Bool 消息 来 启动 
仿真 。 

(2) pauseSimulation 主题 : 可 以 用 于 通过 发 布 一 个 std_msgs::Bool 消息 来 暂停 仿真 。 

(3) stopSimulation 主题 : 可 以 用 于 通过 发 布 此 主题 std_msgs::Bool 消息 来 停止 仿真 。 

(4) enableSyncMode 主题 : 通过 发 布 关 于 此 主题 的 std_msgs::Bool 消息 ,可 以 启用 / 
禁用 同步 仿真 模式 。 

(5) triggerNextStep 主题 : 通过 发 布 关 于 此 主题 的 std_msgs::Bool 消息 ,可 以 在 同步 
仿真 模式 下 触发 下 一 个 仿真 步骤 。 

(6) simulationStepDone 主题 ; 类 型 std_msgs::Bool 的 消息 将 在 每 个 仿真 传递 结束 时 
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发 布 。 

(7) simulationState 主题 : std_msgs::Int32 类 型 的 消息 将 定期 发 布 。 其 中 ,0 表示 仿真 
停止 ,1 表示 仿真 正在 运行 ,2 表示 仿真 已 暂停 。 

(8) simulationTime 主题 : std_msgs::Float32 类 型 的 消息 将 定期 发 布 , 指 示 当 前 的 仿 
真 时 间 。 

可 以 查看 完全 自 定义 的 脚本 内 容 。 尝 试 从 命令 行 生成 主题 消息 ,例如 : 


rostopic pub / startSimulation std msgs / Bool true —- once 
rostopic pub / pauseSimulation std msgs / Bool true —- once 
rostopic pub / stopSimulation std msgs / Bool true —- once 
rostopic pub / enableSyncMode std msgs / Bool true —- once 
rostopic pub / startSimulation std msgs / Bool true —- once 
rostopic pub / triggerNextStep std msgs / Bool true —- once 
rostopic pub / triggerNextStep std msgs / Bool true —- once 
rostopic pub / triggerNextStep std msgs / Bool true —- once 
rostopic pub / stopSimulation std msgs / Bool true —- once 


n 40 40 40 4B 40 X5 X0 40 


为 了 显示 当前 的 仿真 时 间 , 可 以 键入 : 


$ rostopic echo / simulationTime 


6.5.5 V-REP ROS Bridge 的 简介 及 安装 


V-REP ROS Bridge 是 由 位 于 Inria Rennes 的 Inria Lagadic 公司 开发 的 V-Reporter 的 
插件 。 插 件 的 主要 目的 是 提供 V-REP f ROS 之 间 的 通信 接口 ,让 用 户 能 够 通过 ROS 消息 
和 ROS 服务 在 外 部 控制 V-REP 仿真 。 

这 个 插件 是 一 个 共享 库 , 由 V-REP 的 主要 客户 端 应 用 程序 在 启动 时 自动 加 载 。 这 一 
插件 可 以 自动 在 场景 中 寻找 已 知 的 对 象 , 以 便 管理 它们 。 它 创建 ROS 发 布 者 发 送 仿 真 数据 
(例如 场景 中 存在 的 对 象 的 姿态 和 速度 ) .接收 (ROS 订阅 者 ) 消 息 ,并 动态 地 将 命令 应 用 到 
对 象 。 请 注意 ,vrep_ros_bridge 是 一 个 元 包 , 而 vrep ros plugin 是 真正 的 主 包 。 

ROS V-Rep Bridge 使 用 pluginlib 包 。Pluginlib 是 一 个 C++ 库 ,用 于 从 ROS 包 中 加 载 
和 钾 载 插件 。 插 件 是 从 运行 时 库 ( 即 共享 对 象 .动态 链接 库 ) 加 载 的 动态 可 加 载 类 。 这 样 我 
们 的 处 理 程序 实际 上 是 一 些 依赖 的 插件 。 如 果 我 们 不 需要 一 个 处 理 程序 ,或 者 我 们 没有 安 
装 它 的 依赖 ,仍然 能 够 建立 桥梁 (该 插件 将 不 可 用 )。 例 如 ,quadrotor_tk_handler 需要 
Telekyb。 如 果 我 们 不 想 安装 Telekyb. 可 以 在 quadrotor tk handler 文件 夹 中 添加 一 个 名 
为 CATKIN_IGNORE 的 文件 ,其 他 处 理 程序 将 可 用 。 

插件 支持 的 对 象 有 机 器 人 和 传感器 : 其 中 机 器 人 包括 臂 型 机 器 人 和 移动 机 器 人 
(manipulator_handler) .四 旋翼 飞行 器 (quadrotor_handler); 传感器 包括 了 视觉 传感器 
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(camera_handler) 和 IMTU 传感器 (imu_handler) 。 
1. V-Rep ROS Bridge 的 安装 步骤 
CD 通过 终端 访问 catkin. ws/src. 
(2) 从 GIT 下 载 插件 : 


git clone https://github. com/lagadic/vrep_ ros_bridge. git; 


G) 如 果 使 用 ROS Indigo 或 ROS Jade, 请 使 用 分 支 主机 。 
(4) 如 果 使 用 Hydro: 


git checkout hydro - devel 


(5) 可 能 不 需要 vrep ros bridge 提供 的 所 有 子 插件 。 一 个 常见 的 情况 是 ,不 需要 使 用 
TeleKyb 框架 。 要 忽略 此 子 插件 ,需要 在 不 需要 的 子 插件 文件 夹 中 添加 文件 CATKIN_ 
IGNORE( 因 为 它 在 quadrotor_tk_handler 中 完成 )。 要 做 到 这 一 点 ,通过 在 sub-plugin 文 
件 夹 中 的 终端 执行 : 


touch CATKIN IGNORE 
(6) 打开 文件 bashre: gedit 一 / . bashrc', 在 文件 未 尾 添 加 : 

export VREP ROOT DIR = / ChangeWithyourPathToVrep / 

Jf B. ,如果 要 避免 每 次 键 和 人, 使 用 以 下 命令 : 

export ROS PACKAGE PATH = $ (ROS PACKAGE PATH]: / path to catkin ws / catkin ws / src 


source/opt/ros/ indigo/setup. bash 
source /path to catkin ws/catkin ws/devel/setup.bash 


(7) 进入 catkin workspace. Jf-i& fT: 


catkin make 


(8) 现在 使 用 下 一 条 指令 再 次 构建 pkg: 
catkin make —— pkg vrep ros bridge -- cmake — args - DCMAKE BUILD TYPE = RelWithDebInfo 
(90 在 文件 夹 catkin_ws/devel/lib/ 中 .将 找到 主 库 (libv_repExtRosBridge. so) 和 其 他 


库 (libcamera_handler. so.libmanipulator handler. so, libquadrotor_ handler. so. librigid _ 


body_handler. so) 。 
(10) 文件 libv_repExtRosBridge. so 必须 在 V-Rep 安装 文件 夹 中 才能 加 载 。 要 做 的 是 
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创建 一 个 符号 链接 。 通 过 终端 进入 V-REP 的 安装 文件 夹 并 输入 : 


ln — s /YOUR_CATKIN WS PATH/devel/lib/libv repExtRosBridge. so 


其 中 ,/YOUR_CATKIN_WS_PATH 是 到 达 工 作 区 的 实际 路 径 。 

(11) 如 果 使 用 的 是 新 版 本 的 V-REP(V3_3_1_64_Linux) ,需要 一 个 额外 的 步 又。 创建 
一 个 链接 ,指向 V-REP 根 文件 夹 中 的 compiledRosPlugins/libv repExtRos. so 文件。 通过 
终端 进入 V-REP 的 安装 文件 夹 并 输入 : 


ln — s compiledRosPlugins / libv repExtRos. so 


2. 测试 

完成 以 上 步骤 后 ,简单 地 测试 是 否 安装 成 功 : 

在 ROS 系统 的 终端 中 键入 roscore ,然后 通过 终端 运行 V-Rep。 
在 终端 的 窗口 ,将 显示 : 


Add - on script 'vrepAddOnScript ~ addonScriptDemo. lua' was loaded. 
Simulator launched. 

f) 

Plugin 'RosBridge': loading... 

Plugin 'RosBridge': load succeeded. 

S) 


这 表明 V-REP ROS Bridge 已 经 正确 安装 。 
关于 V-Rep ROS Bridge 的 更 多 信息 ,有 兴趣 的 读者 可 以 查看 以 下 网 址 : https:// 
github. com/lagadic/vrep ros bridge. 


6.6 ROS 5 Gazebo 


ROS 是 一 个 机 器 人 控制 框架 。 本 节 将 讲述 如 何 建立 Gazebo 和 ROS 的 接口 。 
6.6.1 ROS 集成 概述 


ROS 可 以 通过 Gazebo ros pkgs (一 组 ROS 包 ) 实 现 独立 的 Gazebo 包装 器 与 独立 运 
行 的 Gazebo 的 集成 。 这 组 ROS 包 利 用 ROS 消息 ,服务 和 动态 重新 配置 ,向 Gazebo 中 的 仿 
真 机 器 人 提供 必要 的 接口 。 

Gazebo ros pkgs 的 一 些 功 能 如 下 : 

。 支持 未 与 ROS 绑 定 的 Gazebo 的 独立 系统 ; 

。 在 catkin 工作 空间 运行 ; 

。 支持 URDF 和 SDF 文件 ; 
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* 减少 Gazebo 重复 代码 ; 

* 改善 了 ros control 对 控制 器 支持 服务 ; 

* 内置 DARPA 机 器 人 挑战 赛 中 的 实时 控制 器 优化 内 容 ; 

。 剔除 一 些 旧 版 ROS 和 Gazebo 代码 。 

升级 simulator_Gazebo( ROS groovy 和 更 早 版 本 ) 时 ,为 了 获得 更 好 的 ROS 包 的 使 用 
体验 ,以 下 是 从 simulator_Gazebo 获取 Gazebo 相关 包 对 软件 进行 升级 的 方法 。 

启动 文件 时 ,最 简便 的 方法 是 在 Gazebo 中 获取 帮助 ,使 用 roslaunch 文件 来 生成 模型 。 
简 而 言 之 : 

在 roslaunch 文件 中 ,将 pkg 二 "Gazebo" 重 命名 为 pkg — "Gazebo ros". MIR Gazebo | 
worlds 包 。 

由 于 大 多 数 world 文件 很 少 使 用 ,并 且 不 支持 SDF XML 格式 的 变化 。 因 此 ,所 有 的 
world 文件 (包括 empty. world) 都 集成 在 Gazebo 项 目 中 。 

使 用 Gazebo 启动 文件 的 最 佳 方法 是 直接 继承 /声明 empty_world 位 于 Gazebo_ros 包 
中 的 主 启动 文件 。 

1. CMakeLists. txt 

删除 ROS 包装 版 本 Gazebo, 有 利于 Gazebo 的 系统 安装 。 这 可 能 需要 重新 配置 CMake 
文件 。 下面 是 一 个 CMakeLists. txt 示例 : 


cmake minimum required(VERSION2.8.3) 
project(YOURROBOT Gazebo plugins) 


find package(catkin REQUIRED COMPONENTS 
Gazebo ros 
) 


# Depend on system install of Gazebo 
find package(Gazebo REQUIRED) 


include directories(include $ (catkin INCLUDE DIRS) $ (GAZEBO INCLUDE DIRS) $ (SDFormat - 
INCLUDE DIRS]) 


* Build whatever you need here 
add library(...) & TODO 


catkin package( 
DEPENDS 

Gazebo ros 
CATKIN DEPENDS 
INCLUDE DIRS 
LIBRARIES 
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2. package. xml 
在 新 Gazebo ros 包 上 添加 依赖 关系 : 


< build depend» Gazebo ros </build depend 
« run depend» Gazebo ros «/run depend» 


3. 运行 Gazebo 

为 了 与 Gazebo 可 执行 文件 名 称 一 致 ,对 启动 Gazebo 的 ROS 节点 的 名 称 作出 细微 的 
改变 : 

rosrun Gazebo_ros Gazebo 启动 Gazebo 服务 器 和 GUI。 


rosrun Gazebo ros gzclient 启动 Gazebo GUI, 
rosrun Gazebo ros gzserver 启动 Gazebo 服务 器 。 


可 运行 节点 : 


rosrun Gazebo ros Gazebo 
rosrun Gazebo ros gzserver 
rosrun Gazebo ros gzclient 
rosrun Gazebo ros spawn model 
rosrun Gazebo ros perf 

rosrun Gazebo ros debug 


6.6.2 安装 Gazebo ros pkgs 


首先 ,需要 安装 Gazebo。 用 户 可 以 从 源 代码 或 从 预 构建 Ubuntu debians 安装 Gazebo。 
如 果 从 源 代 码 安装 ,请 确保 先 构 建 Gazebo_X.Y 分 支 (X,Y 分 别 代表 用 户 所 需 的 版 本 ) 。 

然后 测试 独立 Gazebo 工作 状况 。 在 安装 Gazebo_ros_pkgs 之 前 ,需要 确保 独立 的 
Gazebo 在 终端 中 和 运行。 测试 方法 如 下 : 单 击 左 侧 的 “插入 ”选项 卡 ,选择 要 添加 的 模型 ， 然 
后 在 simulation 上 的 目标 位 置 上 单 击 ,确定 模型 位 置 。 

最 后 测试 Gazebo 版 本 。 确 保 Gazebo 安装 在 正确 的 位 置 。 请 运行 ; 


which gzserver 
which gzclient 


如 果 从 source 中 安装 到 默认 位 置 ,会 出 现 以 下 信息 : 


/ usr / local / bin / gzserver 
/ usr / local / bin / gzclient 


如 果 用 户 从 debian 安装 ,会 出 现 : 
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/ usr / bin / gzserver 
/ usr / bin / gzclient 


1. 3€ Gazebo ros pkgs 

安装 Gazebo ros pkgs 有 两 种 方法 ,用 户 可 以 自行 选择 一 种 合适 的 方法 。 从 软件 包 安 
装 的 优点 是 过 程 便 捷 ; 而 从 源 代码 安装 的 优点 则 是 可 以 更 方便 地 调试 和 修改 。 

1) 安装 预 构建 的 Debian 

可 使 用 Gazebo_ros_pkgs 软件 包 。 安 装 指令 如 下 : 

对 于 ROSKinetic 版 本 : 


Sudoapt - get install ros - kinetic - Gazebo - ros - pkgsros - kinetic - Gazebo - ros - control 


对 于 ROS Jade 版 本 : 


sudo apt - get install ros - jade - Gazebo - ros - pkgs 


对 于 ROS Indigo 版 本 : 


sudo apt - get install ros- indigo- Gazebo - ros - pkgs ros - indigo - Gazebo - ros - control 


如 果 安 装 成 功 ,请 跳 到 下 面 的 “使 用 ROS 集成 测试 Gazebo” 部 分 的 内 容 。 

2) 从 源 代 码 安装 (在 Ubuntu 上 ) 

如 果 用 户 正 在 使 用 早期 版 本 的 ROS(Groovy 或 更 早 版 本 ), 则 需要 选择 从 源 代码 安装 
的 方式 。 若 用 户 要 开发 新 插件 或 提交 修补 程序 ,此 方法 也 十 分 适用 。 安 装 步骤 如 下 : 

O 设置 Catkin 工作 区 。 这 些 指 令 需 要 在 catkin 构建 的 系统 下 运行 。 如 果 没 有 catkin 
工作 区 设置 ,请 尝试 以 下 命令 : 

mkdir -p —/catkin ws/src 

cd —/catkin ws/src 

catkin init workspace 

cd —/catkin ws 

catkin make 


然后 ,添加 源 代码 到 . bashrc 文件 的 安装 脚本 : 


echo "source ~/catkin ws/devel/setup. bash" >> 一 /.bashrc 


© 复制 Github 存储 库 。 首 先 确保 git 安装 在 用 户 的 Ubuntu 计算 机 上 ,然后 输入 以 下 
vA, 


指令 : 


sudo apt - get install git 
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对 于 ROSKinetic 版 本 : 
sudo apt- get install — y libGazebo7 — dev 


从 Gazebo ros pkgsgithub 存储 库 下 载 源 代码 : 


cd —/catkin ws/src 
git clone https: //github.com/ros - simulation/Gazebo ros pkgs.git - b kinetic - devel 


使 用 rosdep 检查 是 否 有 丢失 的 依赖 关系 : 


rosdep update 
rosdep check -- from- paths. -- ignore- src -- rosdistro kinetic 


你 可 以 使 用 rosdep 通过 debian 安装 自动 安装 时 缺失 的 依赖 关系 : 


rosdep install -- from- paths . -- ignore- src -- rosdistro kinetic -y 


对 于 ROSJade 版 本 : 
Jade 使 用 Gazebo 5. x 系列 ,安装 指令 如 下 : 


sudo apt- get install - y libGazebo5 - dev 


cd —/catkin ws/src 
git clone https://github.con/ros - simulation/Gazebo ros pkgs.git - b jade - devel 


使 用 rosdep 检查 是 否 有 丢失 的 依赖 关系 : 


rosdep update 
rosdep check -- from- paths. -- ignore- src -- rosdistro jade 


你 可 以 使 用 rosdep ,然后 通过 debian 安装 自动 安装 缺失 的 依赖 关系 : 
rosdep install -- from- paths . -- ignore- src -- rosdistro jade - y 


注意 : 目前 在 ROS Jade 中 没有 发 布 ros-jade-Gazebo-ros-control 包 。 请 检查 Gazebo | 
ros control 跟踪 器 中 的 问题 以 查看 进度 。 同 时 ,需要 禁用 Gazebo-ros-control: 


touch Gazebo ros pkgs/Gazebo ros control/CATKIN IGNORE 


对 于 ROS Indigo 版 本 : 
Indigo 使 用 Gazebo 2. x 系列 ,安装 指令 如 下 : 


580 «(| 机 器 人 仿真 与 编程 技术 


sudo apt 一 get install - y Gazebo2 
cd —/catkin ws/src 
git clone https: //github.com/ros - simulation/Gazebo ros pkgs.git - b indigo- devel 


使 用 rosdep 检查 是 否 有 丢失 的 依赖 关系 : 


rosdep update 
rosdep check -- from- paths. -- ignore- src —- rosdistro indigo 


你 可 以 使 用 rosdep ,通过 debian 安装 自动 安装 缺失 的 依赖 关系 : 


rosdep install -- from- paths . -- ignore- src -- rosdistro indigo -y 


© 构建 Gazebo_ros_pkgs。 要 构建 Gazebo ROS 集成 软件 包 ,请 运行 以 下 命令 : 


cd 一 /catkin_ws/ 
catkin make 


2. 使 用 ROS 集成 测试 Gazebo 
确保 始终 提供 适当 的 ROS 设置 文件 ,如 对 Kinetic 应 该 输入 以 下 指令 : 


source /opt/ros/kinetic/setup. bash 


假设 你 的 ROS 和 Gazebo 环境 已 正确 设置 和 构建 。 启 动 roscore 后 ,你 应 该 能 够 通过 
rosrun 命令 运行 Gazebo。 如 果 它 不 在 用 户 的 . bashrc 里 ,请 搜索 catkin setup. bash: 


source ~ /catkin ws/devel/setup. bash 
roscore& 
rosrun Gazebo ros Gazebo 


Gazebo 用 户 界 面 中 的 窗口 中 不 显示 任何 内 容 。 
若 要 验证 是 否 设置 了 正确 的 ROS 连接 ,请 运行 以 下 指令 查看 可 用 的 ROS 主题 : 


rostopic list 


用 户 应 该 在 列表 中 看 到 以 下 主题 : 


/ Gazebo / link_states 

/ Gazebo / model states 

/ Gazebo / parameter descriptions 
/ Gazebo / parameter updates 

/ Gazebo / set link state 

/ Gazebo / set model state 
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户 还 可 以 验证 Gazebo 服务 是 否 存在 : 


rosservice list 


户 可 以 在 列表 中 看 到 如 下 服务 : 


/ Gazebo / apply body wrench 

/ Gazebo / apply joint effort 

/ Gazebo / clear body wrenches 

/ Gazebo / clear joint forces 

/ Gazebo / delete model 

/ Gazebo / get. joint properties 

/ Gazebo / get link properties 

/ Gazebo / get link state 

/ Gazebo / get loggers 

/ Gazebo / get model properties 

/ Gazebo / get model state 

/ Gazebo / get physics properties 
/ Gazebo / get world properties 

/ Gazebo / pause physics 

/ Gazebo / reset simulation 

/ Gazebo / reset world 

/ Gazebo / set joint properties 
/ Gazebo / set link properties 

/ Gazebo / set link state 

/ Gazebo / set logger level 

/ Gazebo / set model configuration 
/ Gazebo / set model state 

/ Gazebo / set parameters 

/ Gazebo / set physics properties 
/ Gazebo / spawn Gazebo model 

/ Gazebo / spawn sdf model 

/ Gazebo / spawn urdf model 

/ Gazebo / unpause physics 

/ rosout / get loggers 

/ rosout / set logger level 


3. 其 他 ROS 启动 Gazebo 方法 
以 下 几 个 rosrun 命令 均 可 以 启动 Gazebo。 
(1) 同时 启动 服务 器 和 客户 端 : 


rosrun Gazebo ros Gazebo 
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(2) 仅 启 动 Gazebo 服务 器 : 


rosrun Gazebo ros gzserver 


(3) BUB 3l Gazebo 客户 端 : 


rosrun Gazebo ros gzclient 


(4) 仅 使 用 GDB 在 调试 模式 下 启动 Gazebo 服务 器 : 


rosrun Gazebo ros debug 


(5) 此 外 ,可 以 使 用 Gazebo 启动 roslaunch: 


roslaunch Gazebo ros empty world. launch 


6.6.3  ROS/Gazebo 版 本 组 合 的 选择 


1. Gazebo 版 本 的 选择 

如 果 用 户 计划 使 用 特定 版 本 的 ROS ,并 且 不 需要 使 用 特定 版 本 的 Gazebo, 那 么 可 以 参 
照 网 页 (http://gazebosim. org/tutorials? tut = ros _installing&.cat 二 connect_ros) 安 装 
Gazebo ros pkgs 内 容 。 本 节 将 介绍 如 何 安 装 ROS 完全 支持 的 Gazebo 版 本 。 

请 注意 ,使 用 与 ROS 存储 库 提供 的 官方 不 同 的 Gazebo 版 本 可 能 会 导致 ROS 包 的 冲突 
或 其 他 集成 问题 。 

2. Gazebo 版 本 和 ROS 集成 

Gazebo 是 一 个 如 boostogre 或 其 他 任意 被 ROS 使 用 的 独立 项 目 。 通 常 , 在 每 个 ROS 
版 本 发 布 时 可 用 的 最 新 版 本 的 Gazebo( 例 如 Gazebo5 对 于 ROS Jade) 会 被 选择 为 完全 集成 
和 支持 该 ROS 版 本 的 正式 版 本 ,并 且 将 会 在 ROS 的 整个 生命 周期 中 得 到 服务 支持 。 

Gazebo 开发 不 与 ROS 同步 ,因此 每 个 新 版 本 的 Gazebo 必须 在 ROS 发 布 使 用 之 前 被 
发 布 。 

3. 安装 Gazebo 

1) Gazebo Ubuntu 包 

安装 Gazebo 的 最 简单 的 方法 是 使 用 软件 包 。 以 下 是 两 个 主要 用 来 托管 Gazebo 软件 
包 的 软件 库 : packages. ros. org 与 packages. osrfoundation. org。 


packages. ros. org 


* Indigo: 主机 Gazebo 版 本 2.x 包 。 
* Jade: 主机 Gazebo 版 5.x 包 。 
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。 Kinetic: 主机 或 使 用 Gazebo 版 本 7.x 包 。 


packages. osrfoundation. org 


。 Gazebo 5. x 系列 ( 包 和 名 Gazebo5) 。 

。 Gazebo 6. x 系列 ( 包 名 Gazebo6)。 

。 Gazebo 7. x 系列 ( 包 名 Gazebo7)。 

这 说 明 包含 osrfoundation 存储 库 不 是 得 到 Gazebo Ubuntu 包 的 必要 条 件 。 用 户 可 以 
从 ros 存储 库 安装 。 

2) 由 源 代码 构建 Gazebo 

如 果 你 从 源码 编译 了 Gazebo 版 本 ,注意 ,根据 使 用 的 存储 库 分 支 (Gazebo6,Gazebo7 
0 ,只 有 当 用 户 的 本 地 分 支 存 储 库 和 ROS 发 行 版 中 使 用 的 Gazebo 版 本 相 匹 配 时 ,用 户 的 
Gazebo 才能 是 兼容 二 进 制 的 Gazebo_ros_pkgs。 注 意 ,如 果 用 户 使 用 默认 版 本 ,那么 用 户 可 
能 与 其 他 一 些 发 布 的 软件 包 不 能 兼容 二 进 制 , 所 以 用 户 需 要 一 个 catkin 工作 区 来 获得 一 个 
有 效 的 Gazebo_ros_pkgs。 

4. 使 用 默认 版 本 的 Gazebo 

对 于 需要 运行 特定 版 本 的 ROS 并 想 要 使 用 所 有 Gazebo ROS 相关 软件 包 的 用 户 ,推荐 
使 用 以 下 ROS 版 本 : 

1) Kinetic 

ROS Kinetic 主机 是 使 用 7. x 版 本 的 Gazebo。 对 于 完全 集成 的 ROS 系统 ,建议 使 用 7. x 
版 本 的 Gazebo。 继 续 进程 的 方法 就 是 使 用 ROS 库 ( 它 会 自动 安装 Gazebo?) ,并 且 还 没有 使 
用 osrfoundation 库 。 

2) Jade 

ROS Jade 托管 5. x 版 本 的 Gazebo。 对 于 完全 集成 的 ROS 系统 ,建议 使 用 5. x 版 本 的 
Gazebo。 继 续 进程 的 方法 就 是 使 用 ROS 库 ( 它 会 自动 安装 Gazebo5), 且 禁止 使 用 
osrfoundation JÆ 。 

3) Indigo 

ROS Indigo 主机 是 2. x 版 本 的 Gazebo。 对 于 完全 集成 的 ROS 系统 ,建议 使 用 2. x 版 
本 的 Gazebo。 继 续 进 程 的 方法 就 是 使 用 ROS 库 ( 它 会 自动 安装 Gazebo2), 并 禁止 使 用 
osrfoundation f£ 。 

5. 使 用 特定 Gazebo 版 本 

使 用 以 下 方法 可 以 安装 使 用 任何 特定 版 本 的 Gazebo 和 ROS, 

1) Gazebo 7. x 系列 

OSRF 存储 库 提供 了 建立 在 软件 包 顶 部 的 -Gazebo7-ROS/Indigo 和 ROS/Jade Gazebo 
包装 器 (Gazebo7_ros_pkgs) 的 Gazebo7 版 本 。 使 用 步骤 如 下 : 

。 将 osrfoundation 存储 库 添加 到 源 列表 。 

* 从 osrfoundation 库 中 安装 ros-$ ROS_DISTRO-Gazebo7-ros-pkgs, 本 次 操作 将 会 
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安装 Gazebo7 包 。 

* 使 用 catkin 工作 区 来 编译 从 源 代 码 得 到 的 其 余 软件 。 

2) Gazebo 6. x 系列 

OSRF 存储 库 提供 了 建立 在 软件 包 顶 部 的 -Gazebo6-ROS/Indigo 和 ROS/Jade Gazebo 
包装 器 (Gazebo6_ros_pkgs) 的 Gazebo6 版 本 。 使 用 步骤 如 下 : 

。 将 osrfoundation 存储 库 添加 到 源 列表 。 

。 从 osrfoundation 库 安 装 ros-$ROS_DISTRO-Gazebo6-ros-pkgs, 本 次 操作 将 安装 

Gazebo6 包 。 

* 使 用 catkin 工作 区 来 编译 从 源 代码 得 到 的 其 余 软件 。 

3) Gazebo 5. x 系列 

OSRF 存储 库 提供 了 建立 在 软件 包 顶 部 的 -Gazebo5-ROS/IndigoGazebo 包装 器 
(Gazebo5_ros_pkgs) 的 Gazebo5 版 本 。 使 用 步骤 如 下 : 

。 将 osrfoundation 存储 库 添 加 到 源 列表 。 

。 从 osrfoundation 库 安装 ros-indigo-Gazebo5-ros-pkgs, 本 次 操作 将 安装 Gazebo5 包 。 

。 使 用 catkin 工作 区 来 编译 从 源 代码 得 到 的 其 余 软 件 。 

6. 几 个 问题 

(1) 如 果 不 使 用 ROS ,应 该 使 用 哪个 版 本 ? 

如 果 用 户 不 需要 ROS 支持 ,推荐 的 版 本 是 最 新 发 布 的 版 本 ,可 以 参照 网 页 (http:// 
gazebosim. org/install) 使 用 osrfoundation repo 安装 。 

(2) 如 果 想 使 用 bullet/simbody/dart 物理 引擎 ,应 该 使 用 哪个 版 本 的 Gazebo? 

Ubuntu 包 内 置 Gazebo4, bullet 和 simbody 服务 支持 。 请 按照 上 述 Gazebo4 说 明 与 
ROS 组 合 使 用 。Dart 仍然 需要 从 源 (Gazebo3) 安 装 Gazebo, 所 以 你 可 以 使 用 Gazebo3 或 更 
高 的 版 本 ,并 按照 上 面 的 说 明 进行 操作 ,使 其 与 ROS 兼容 。 

(3) 如 果 需 要 使 用 Gazebo5/Gazebo6/Gazebo7 和 ROSIndigo ,应 该 怎么 做 ? 

如 果 用 户 需要 使 用 一 些 只 出 现在 5.x 版 .6.x 版 或 7.x 版 中 的 功能 ,可 以 参照 如 何 使 用 
ROS 与 Gazebo4 .Gazebo5 或 Gazebo6 的 说 明文 档 安装 Gazebo5 Gazebo6 或 者 Gazebo? 和 
ROSIndigo。 

(4) 如 果 需 要 使 用 Gazebo6/Gazebo7 fil ROS Jade, 应 该 怎么 做 ? 

如 果 用 户 需要 使 用 一 些 只 存在 于 Gazebo 版 本 6.x 或 7.x 中 的 功能 ,其 中 一 种 方式 是 安 
装 Gazebo6 或 Gazebo7 ROS Jade。 请 参照 文档 中 关于 如 何 使 用 ROS 与 Gazebo6 软件 包 的 
说 明 。 

(5) 解决 一 些 ROS 包 与 GazeboX ROS Wrappers 冲突 问题 。 

按照 官方 的 设计 思路 ,每 个 ROS 分 布 与 特定 版 本 的 Gazebo(Gazebo5 在 Jade 中 ) 需 要 
一 起 使 用 。 当 选择 使 用 与 ROS 发 行 版 中 推荐 的 版 本 不 同 的 Gazebo 版 本 时 ,可 能 会 出 现 问 
题 ,而 且 , 其 中 一 些 问题 可 能 无 法 解决 。 如 果 在 尝试 安装 本 文档 中 描述 的 某 个 版 本 之 后 发 现 
依赖 性 冲突 (例如 使 用 RVIZ) ,用 户 将 需要 从 源 代码 安装 ROS 或 Gazebo。 
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6.6.4 使 用 roslaunch 


有 很 多 方法 都 可 以 用 来 启动 Gazebo、 打 开 world 模型 并 将 机 器 人 模型 生成 到 仿真 环境 
中 。 本 节 将 介绍 两 种 方式 : 使 用 rosrun 和 roslaunch。 这 包括 将 用 户 的 URDF 文件 存储 在 
ROS 包 中 ,并 保持 用 户 ROS 工作 区 各 种 资源 相对 路 径 。 

1. 使 用 roslaunch 打开 world 模型 

roslaunch 工具 是 ROS 系统 的 标准 方法 ,可 以 用 于 启动 新 的 ROS 节点。 只 需 运 行 该 工 
具 就 可 以 启动 一 个 类 似 于 在 rosrun 上 输入 下 列 指令 所 产生 的 空 的 Gazebo world: 


roslaunch Gazebo ros empty world. launch 


1) roslaunch 参数 

用 户 可 以 将 以 下 参数 附加 到 启动 文件 以 更 改 Gazebo 的 行为 : 

。 和 暂停: 启动 Gazebo 处 于 暂停 状态 (默认 为 false? 。 

* use sim time; 告诉 ROS 节点 要 求 获得 Gazebo 发 布 的 仿真 时 间 , 通 过 ROS. 主题 / 
时 钟 发 布 ( 默 认为 true) 。 

* gui: 启动 Gazebo 的 用 户 界 面 窗口 (默认 为 true). 

。 headless: 禁用 对 仿真 器 泻 染 (Ogre) 组 件 的 任何 函数 调用 。 不 使 用 gui := true( 默 
认为 false)。 

* debug: 在 调试 模式 下 使 用 gdb 启动 gzserver(Gazebo Server, 默 认为 false) 。 

2) roslaunch 命令 示例 

通常 ,这 些 参 数 的 默认 值 都 是 用 户 需 要 调整 的 ,以 下 仅 作为 示例 : 


roslaunch Gazebo ros empty_world. launch paused : = true use sim time: = false gui: = true 
throttled : = false headless : = false debug: = true 


3) 启动 其 他 演示 world 模型 
Gazebo_ros 软件 包 中 已 包含 其 他 演示 版 本 ,其 中 包括 : 


roslaunch Gazebo ros willowgarage world. launch 
roslaunch Gazebo ros mud world. launch 
roslaunch Gazebo ros shapes world. launch 
roslaunch Gazebo ros rubble world. launch 


启动 文件 mud. world. launch 包含 以 下 内 容 : 


<launch> 

<!—— We resume the logic in empty world. launch, changing only the name of the world to be 
launched 一 一 > 

< include file="$ (find Gazebo ros)/launch/empty world. launch"> 
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< arg name = "world name" value = "worlds/mud. world"/> <! —— Note: the world name is with 
respect to GAZEBO RESOURCE PATH environmental variable 一 一 > 

« arg nane = "paused" value = "false"/» 

«arg name = "use sim time" value = "true"/> 

«arg name = "gui" value = "true" /> 

« arg name = "headless" value = "false" /> 

«arg name = "debug" value = "false"/» 

x/ include 

</launch> 


在 这 个 启动 文件 中 ,从 empty_world. launch 继承 了 大 部 分 必要 的 功能 。 需 要 改变 的 唯 


一 参数 是 world name 参数 ,其 他 参数 设置 为 其 默认 值 。 


4) world 文件 
接 下 来 介绍 mud. world. launch 文件 。 首 先 查看 mud. world 文件 的 内 容 , 文 件 的 前 几 


个 组 件 如 下 所 示 : 


到 。 


< sdf version- "1.4"> 

X world name = "default"> 

< include» 

X uri» model://sun «/uri? 

«/ include» 

< include» 

« uri» model://ground plane </uri> 
«/ include» 

< include» 

«uri» model://double pendulum with base </uri> 
< name» pendulum thick mud </name > 
«pose» -2.0000 00 «/pose» 

X/ include» 


</world> 
</sdf > 


在 这 个 world 文件 片段 中 引用 了 3 个 模型 ,可 以 在 本 地 Gazebo 模型 数据 库 中 搜索 得 
如 果 搜 索 不 到 ,它们 会 自动 从 Gazebo 的 在 线 数据 库 中 拉 出 。 用 户 可 以 在 网 页 (http:// 


gazebosim. org/tutorials? cat 二 build_world) 中 了 解 有 关 world 文件 的 更 多 信息 。 


A) 如 何在 计算 机 上 查找 world 文件 。 
world 文件 位 于 Gazebo 资源 路 径 的 /worlds 目录 中 。 此 路 径 的 位 置 取决 于 用 户 如 何 安 


装 Gazebo 和 用 户 的 系统 类 型 。 可 以 使 用 以 下 命令 查找 Gazebo 资源 的 位 置 : 


env | grep GAZEBO RESOURCE PATH 


典型 的 路 径 可 能 是 类 似 于 : /usr/local/share/Gazebo-l. 9。 在 这 个 路 径 结 尾 添 加 


/worlds, 此 目录 包含 了 Gazebo 使 用 的 world 文件 ,包括 mud. world 文件 。 
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(2) 如 何 创建 自己 的 Gazebo ROS 包 。 

在 继续 讲述 如 何 将 机 器 人 模型 生成 到 Gazebo 之 前 .首先 讨论 使 用 ROS 和 Gazebo 的 文 
件 层次 结构 标准 。 

现在 ,将 假设 用 户 的 catkin 工作 区 被 命名 catkin_ws( 用 户 也 可 以 命名 为 其 他 名 字 )。 用 
户 的 catkin 工作 区 可 能 位 于 用 户 的 计算 机 上 ,如 : 


/ home / user / catkin ws / src 


关于 机 器 人 的 模型 和 描述 的 所 有 内 容 , 根 据 ROS 标准 ,位 于 一 个 名 为 /MYROBOT_ 
description 的 包 中 ,所 有 的 world 文件 和 Gazebo 使 用 的 启动 文件 位 于 一 个 名 为 
/MYROBOT Gazebo 的 ROS 包 中 。 使 用 这 两 个 包 , 用 户 的 层次 结构 如 下 所 示 : 


../catkin ws/src 
/MYROBOT description 
package. xml 
CMakeLists.txt 
/urdf 
MYROBOT. urdf 
/neshes 
meshl.dae 
mesh2. dae 


/naterials 
/cad 
/MYROBOT Gazebo 
/launch 
MYROBOT. launch 
/worlds 
MYROBOT. world 
/models 
world objectl.dae 
world object2.stl 
world object3.urdf 
/naterials 
/plugins 


注意 : 命令 catkin create pkg 用 于 创建 新 包 。 

下 面 介 绍 如何 自 行 设置 并 使 用 自 定义 的 world 文件 。 

(3) 创建 自 定义 world 文件。 

日 户 可 以 在 自己 的 ROS 包 中 创建 特定 的 world 文件 以 及 其 他 的 自 定义 文件 。 下 面 的 
例子 将 创建 一 个 空地 面 、 一 个 地 面 、 一 个 太阳 和 一 个 加 油 站 。 假 设 : 首先 确保 将 
MYROBOT 替换 为 用 户 的 机 器 人 的 名 称 , 如 果 用 户 并 没有 用 于 测试 的 机 器 人 , 则 只 需 将 其 
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替换 为 类 似 “test” 的 东西 : 
(D 创建 一 个 使 用 约定 MYROBOT_Gazebo ff] ROS 包 。 
© 在 此 包 中 ,创建 一 个 launch 文件 夹 。 
© 在 launch 文件 夹 中 创建 一 个 具有 以 下 内 容 ( 缺 省 参数 排除 ) 的 YOUROBOT. launch 


文件 
< launch> 
X! -- We resume the logic in empty world. launch, changing only the name of the world to be 
launched 一 一 > 


< include file =" $ (find Gazebo ros)/launch/empty world. launch"> 

«arg name = "world name" value =" $ (find MYROBOT Gazebo)/worlds/MYROBOT. world" /> 
<! — more default parameters can be changed here -一 > 

«/ include» 

</launch> 


@ 在 同一 个 包 中 ,创建 一 个 worlds 文件 夹 ,并 创建 一 个 MYROBOT. world 文件 ,包含 
以 下 内 容 : 


<?xml version= "1.0" ?> 

< sdf version- "1.4"> 

< world name = "default"> 

< include» 

X uri? model://ground plane </uri> 
X/ include» 

< include» 

X uri? model://sun «/uri? 

X/ include» 

< include? 

X uri» model://gas station </uri > 
< name > gas station </name > 
<pose>- 2.0 7.000 0 0</pose> 
</include> 

</world> 

</sdf > 


C) 现在 ,用 户 可 以 使 用 以 下 命令 启动 自 定义 world( 加 油 站 ) 到 Gazebo: 


. ~/catkin_ws/devel/setup. bash 
roslaunch MYROBOT Gazebo MYROBOT. launch 


如 图 6-11 所 示 ,可 以 看 到 以 下 world 模型 (使 用 鼠标 上 的 滚轮 可 以 进行 缩放 操作 ) ; 
(4) 在 Gazebo 中 编辑 world 文件 。 
户 可 以 将 其 他 模型 插入 到 机 器 人 的 world 文件 中 ,并 使 用 File 菜单 中 的 SaveAs 命 
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图 6-11 world 模型 


令 将 编辑 的 world 模型 导出 到 ROS 包 。 

2. 使 用 roslaunch 生成 URDF 机 器 人 

使 用 roslaunch 在 Gazebo 中 启动 基于 URDF 的 机 器 人 有 以 下 两 种 方法 。 

(D ROS 服务 调用 方法 。 这 种 方法 的 优点 是 使 用 户 的 机 器 人 ROS 包 在 计算 机 和 存储 
库 检 查 之 间 更 加 具有 可 移植 性 。 它 允许 用 户 保持 用 户 的 机 器 人 的 位 置 相对 于 一 个 ROS 包 
路 径 , 但 也 需要 用 户 使 用 一 个 小 脚本 进行 ROS 服务 调用 。 

(2) 模型 数据 库 方法 。 这 种 方法 的 优点 是 允许 用 户 将 机 器 人 包括 在 . world 文件 中 ,这 
看 起 来 更 便捷 ,但 需要 通过 设置 环境 变量 将 用 户 的 机 器 人 添加 到 Gazebo 模型 数据 库 。 下 
文 将 分 别 讨论 这 两 种 方法 ,推荐 使 用 "ROS 服务 调用 产生 方法 ”。 

1)“ROS 服务 调用 ”机 器 人 生成 方法 

这 个 方法 使 用 一 个 小 的 python 脚本 spawn_model 来 向 Gazebo_rosROS 节点 (在 
rostopic 命名 空间 中 简称 为 “Gazebo”) 发 出 服务 调用 请 求 ,并 将 自 定义 URDF 添加 到 
Gazebo 中 。 

用 户 可 以 按 以 下 方式 使 用 此 脚本 : 


rosrun Gazebo ros spawn model — file 'rospack find MYROBOT description'/urdf/MYROBOT. urdf 
—urdf -x0 -y0 -z1 - model MYROBOT 


要 查看 所 有 可 用 的 参数 ,包括 spawn. model 命名 空间 , trimesh Ji fE , E 5 fi ERI RPY 
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方向 可 运行 以 下 指令 : 
rosrun Gazebo ros spawn model -h 


2) URDF 示例 与 Baxter 机 器 人 

如 果 用 户 没 有 一 个 URDF 来 测试 (如 下 例 ) .用户 可 以 从 Rethink Robotics 的 Baxter 机 
器 人 _common( 网 址 为 https://github. com/RethinkRobotics/Baxter 机 器 人 _common) (x 
库 下 载 Baxter 机 器 人 _description 包 。 将 此 软件 包 放 入 catkin 工作 区 中 ,具体 方法 是 运行 


指令 ， 


git clone https://github. com/RethinkRobotics/Baxter 机 器 人 _common. git 


用 户 现 在 应 该 有 一 个 名 为 Baxter 机 器 人 . urdf 位 于 Baxter 机 器 人 _description/urdf/ 
中 的 URDF 文件 ,用 户 可 以 运行 : 


rosrun Gazebo ros spawn model - file 'rospack find Baxter 机 器 人 _description'/urdf/Baxter 机 
器 人 .urdf -urdf -z 1 - model Baxter 机 器 人 


如 图 6-12 所 示 ,用 户 应 该 会 看 到 Baxter 机 器 人 的 模型 。 


图 6-12 Baxter 机 器 人 虚拟 模型 


要 将 其 直接 集成 到 ROS 启动 文件 中 .需要 重新 打开 该 文件 MYROBOT |. Gazebo/ 
launch/ YOUROBOT. launch, 并 在 </launch > 之 前 添加 以 下 内 容 : 
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X! —— Spawn a robot into Gazebo 一 一 > 
< node name = "spawn urdf" pkg = "Gazebo ros" type= "spawn model" args =" - file $ (find Baxter 
机 器 人 _description)/urdf/Baxter robot.urdf - urdf -z 1 —- model Baxterrobot" /> 


启动 此 文件 ,用户 可 以 看 到 与 使 用 rosrun 时 相同 的 结果 。 

3) 运用 PR2 的 XACRO 示例 

如 果 用 户 的 URDF 不 是 XML 格式 ,而 是 XACRO 格式 ,用 户 可 以 对 启动 文件 进行 类 似 
的 修改 。 用 户 可 以 通过 安装 此 程序 包 来 运行 PR2 示例 : 


ROSJade: 
sudo apt - get install ros - jade - pr2 - common 


然后 将 其 添加 到 用 户 在 之 前 创建 的 启动 文件 : 
<! — Convert an xacro and put on parameter server 一 一 > 


«param name = " robot description" command =" $ (find xacro)/xacro. py $ (find pr2 _ 
description) /robots/pr2.urdf.xacro" /> 


<! —— Spawn a robot into Gazebo 一 一 > 
<node name = "spawn urdf" pkg = "Gazebo ros" type = "spawn model" args =" - param robot _ 
description — urdf — model pr2" /> 


启动 这 个 文件 ,可 以 看 到 在 加 油 站 的 PR2, 如 图 6-13 所 示 。 


图 6-13 PR2 机 器 人 虚拟 模型 
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注意 : 在 这 个 部 分 内 容 中 ,由 于 Gazebo API 的 变化 ,仍然 有 很 多 错误 和 警告 从 控制 台 
输出 ,需要 从 PR2 的 URDF 修正 。 

4)“ 模 型 数据 库 ? 机 器 人 生成 方法 

该 方法 允许 用 户 将 机 器 人 包括 在 . world 文件 中 ,这 看 起 来 更 方便 ,但 需要 通过 设置 环 
境 变量 将 用 户 的 机 器 人 添加 到 Gazebo 模型 数据 库 。 因 为 ROS 依赖 关系 从 Gazebo 中 分 离 
出 来 ,所 以 这 个 环境 变量 是 必需 的 。 因 为 Gazebo 没有 ROS 包 的 概念 ,所 以 URDF 包 路 径 
不 能 直接 在 . world 文件 内 使 用 。 

要 使 用 此 方法 ,用 户 必 须 创 建 一 个 仅 包 含 单个 robot 的 新 模型 数据 库 。 这 不 是 将 
URDF 加 载 到 Gazebo 中 的 最 简便 的 方法 ,但 是 不 必 在 用 户 的 计算 机 上 保留 两 个 机 器 人 
URDF 副本 。 

假设 ROS 工 作 空间 文件 层次 结构 按 上 述 所 述 进行 设置 。 唯 一 的 区 别 是 ,现在 一 个 
model. config 文件 添加 到 用 户 的 MYROBOT_description 包 : 


../catkin ws/src 
/MYROBOT description 
package. xnl 
CMakeLists.txt 
model.config 


/naterials 
/ plugins 
/cad 


此 层次 结构 特别 适用 于 通过 以 下 文件 夹 /文件 用 作 Gazebo 模型 数据 库 : 

e /home/user/catkin workspace/src Gazebo 模型 数据 库 的 位 置 ; 

e /MYROBOT_description 一 一 单个 Gazebo 模型 文件 夹 ; 

* model. config Gazebo 在 其 数据 库 中 查找 此 模型 所 需 的 配置 文件 ; 

* MYROBOT. urdf 用 户 的 机 器 人 描述 文件 ,也 被 Rviz, Movelt! 等 使 用 ; 
. stl 或 . dae 文件 放 在 此 处 ,如 同 常规 URDF 一 样 。 


* /meshes 

5) model. config 

每 个 模型 必须 在 其 根 目录 中 有 一 个 model. config 文件 ,其 中 包含 有 关 模 型 的 元 信息 。 
将 其 复制 到 一 个 model. config 文件 中 ,将 用 户 的 文件 名 替换 model. urdf : 


<?xml version= "1.0"?> 
<model> 


第 6 章 ”机 器 人 操作 系统 的 基础 | 其 593 


< name > MYROBOT </name > 
<version>1.0</version> 
< sdf > urdf /MYROBOT. urdf </sdf > 
< author > 
< name > My name </name > 
< email > name@ email. address </email > 
</author > 
<description> 

A description of the model 
</description> 
</model > 


与 SDF 不 同 , 当 用 于 URDFs 时 ,不 同 版 本 不 需要 进行 标记 。 有 关 详 细 信息 请 参阅 
Gazebo 模型 数据 库 文档 。 

6) 环境 变量 

最 后 ,用户 需要 向 . bashrc 文件 添加 一 个 环境 变量 ,告诉 Gazebo 在 哪里 查找 模型 数据 
库 。 首 先 使 用 编辑 器 编辑 “一 /. bashrc”。 然 后 检查 用 户 是 否 已 定义 GAZEBO_MODEL_ 
PATH。 如 果 用 户 已 经 有 一 个 , 则 再 后 边 附 加 分 号 ,否则 添加 新 的 定义 。 假 设 用 户 的 Catkin 
工作 区 在 一 /catkin_ws/ 用 户 的 路 径 : 


export GAZEBO MODEL PATH- /home/user/catkin ws/src/ 


7) 在 Gazebo 中 手动 观看 
现在 测试 用 户 的 新 Gazebo 模型 数据 库 是 否 通过 启动 Gazebo 正确 配置 : 


Gazebo 


然后 单 击 左 侧 的 * 插 入" 标签。 用户 可 能 会 看 到 几 个 不 同 的 下 拉 列 表 , 表 示 系 统 上 可 用 的 不 
同 模型 数据 库 ( 包 括 在 线 数据 库 ) 。 找 到 与 用 户 的 机 器 人 对 应 的 数据 库 ,打开 子 菜单 , 单 击 用 
户 的 机 器 人 的 名 称 , 然 后 使 用 用 户 的 鼠标 在 Gazebo 中 选择 一 个 位 置 放置 机 器 人 o 

8) 在 Gazebo 中 查看 -roslaunch 使 用 模型 数据 库 

模型 数据 库 方法 的 优点 是 : 用 户 可 以 将 机 器 人 直接 包含 在 world 文件 中 ,而 无 须 使 用 
ROS 包 路 径 。 我 们 将 使 用 "创建 world 文件 ”部 分 中 相同 的 设置 ,但 需要 修改 world 文件 。 

在 同一 MYROBOT description/launch 文件 夹 中 ,使 用 以 下 内 容 编 MYROBOT. world 
文件 : 


<?xml version= "1.0" ?> 
<sdf version= "1.4"> 
<world name = "default"> 
<include> 


594 4| 机 器 人 仿真 与 编程 技术 


< uri» model://ground plane </uri> 
</include > 

< include> 

€ uri» model://sun «/uri» 

«/ include» 

< include» 

«uri» model://gas station </uri> 
«name» gas station </name> 
<pose>- 2.0 7.000 0 0 «/pose? 
«/ include» 

< include» 

< uri > model: //MYROBOT «/uri > 
</include> 

</world> 

</sdf > 


现在 用 户 能 够 使 用 以 下 命令 将 加 油 站 和 机 器 人 的 自 定义 世界 启动 到 Gazebo 中 : 


roslaunch MYROBOT Gazebo MYROBOT. launch 


这 种 方法 的 缺点 是 ,用 户 的 打包 MYROBOT. description. MYROBOT | Gazebo 并 不 容 
易 在 计算 机 之 间 移 植 ,用 户 必 须 先 设置 GAZEBO_MODEL_PATH, 才 能 使 用 这 些 
ROS fi , 

从 package. xml 导出 模型 路 径 的 语句 如 下 : 


< export > 

< Gazebo_ros Gazebo_model_path = " $ {prefix}/models"/> 
< Gazebo ros Gazebo media path="$ (prefix)/models"/» 
</export > 


'$ {prefix)' 是 用 户 不 会 立即 知道 的 信息 ,但 在 这 里 是 必要 的 。 

还 有 一 些 有 用 的 信息 ,例如 : 如 何 从 ROS 侧 调 试 这 些 路 径 、 用 户 可 以 使 用 指令 rospack 
plugins -- attrib = "Gazebo media path" Gazebo_ros 检查 将 被 Gazebo 使 用 的 媒体 路 径 。 

现在 用 户 知道 如 何 创建 roslaunch 打开 Gazebo、world 文件 和 URDF 模型 的 文件 ,现在 
用 户 可 以 使 用 A URDF 在 Gazebo 创建 自己 的 Gazebo-ready URDF 模型 。 


6.6.5 ”ROS 通信 
Gazebo 提供 了 一 组 ROS API, 用 于 修改 和 获取 有 关 仿 真 世界 各 个 方面 的 信息 。 本 节 
将 演示 一 些 用 于 操作 仿真 世界 和 对 象 的 实用 程序 。 用 户 也 可 以 在 这 里 找到 ROS 消息 和 


Gazebo 服务 的 完整 列表 。 
刚体 对 象 的 姿态 和 腕 部 被 称 为 其 "状态 ”。 同 时 .对象 具 有 内 在 的 属性 ,例如 质量 和 摩擦 
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系数 。 在 Gazebo 中 "身体 ”是 指 刚性 体 , 与 URDF 上 下 文中 的 “ 连 杆 ” 同 义 。Gazebo 中 的 
“模型 "是 由 “关节 ”连接 的 身体 的 集合 。 

封装 于 Gazebo ros 的 Gazebo_ros_api_plugin 插件 用 于 初始 化 “Gazebo”ROS 节点 。 它 
集成 了 ROS 回调 程序 (消息 传递 ) 与 Gazebo 的 内 部 调度 程序 ,提供 了 下 面 的 ROS 接口 。 此 
ROS API 使 用 户 能 够 通过 ROS 设置 虚拟 环境 的 各 种 属性 。 同 时 能 够 对 环境 中 模型 的 状态 
进行 生成 和 内 省 。 


Gaz 


ebo ros 程序 包 中 提供 了 一 个 名 为 Gazebo ros paths plugin 的 辅助 插件 ,帮助 


Gazebo 找到 ROS 资源 , 即 解析 ROS 包 路 径 名 。 这 个 插件 加 载 了 gzserver 和 gzclient。 
1. Gazebo 已 发 布 参数 


参数 : /use_sim_time: Bool 


通知 ROS 使 用 由 /clock 主题 发 布 的 ROS 时间。 


如 果 通 过 /use_sim_time 参数 使 用 仿真 时 间 ,Gazebo 会 通过 ROS 参数 服务 器 通知 其 他 
应 用 程序 ,特别 是 Rviz。 用 户 启动 Gazebo_ros 时 ,Gazebo 自动 设置 为 true。 

如 果 Gazebo_ros 使 用 主题 为 /clock 的 ROS 系统 提供 的 仿真 同步 时 间 , 则 /use_sim_ 
time 为 true。 有 关 仿 真 时 间 的 更 多 信息 ,请 参阅 网 页 (http://www. ros. org/wiki/roscpp/ 
Overview/Time) 。 

要 查看 设置 参数 的 指令 为 : rosparam get /use_sim_time。 

2. Gazebo 订阅 的 主题 

主题 包括 : 

用 于 设置 连 杆 的 状态 (姿势 / 腕 部 ): 


~/set_ link state: Gazebo msgs/LinkState 


用 于 设置 模型 的 状态 (姿势 / 腕 部 ): 


~/set model state: Gazebo msgs/ModelState 


主题 可 用 于 快速 设置 模型 的 姿势 和 腕 部 ,无 须 等 待 程序 完成 姿势 动作 设置 。 为 此 ,我 们 
需 将 所 需 的 模型 状态 消息 发 布 到 /Gazebo/set_model_state 主题 。 例 如 ,要 通过 主题 设置 测 


可 以 通过 从 在 线 数据 库 中 生成 新 模型 并 将 焦点 添加 到 模型 中 : 


rosrun Gazebo ros spawn model - database coke can - Gazebo - model coke can - y 1 


3l it Az fi /Gazebo/set model state 3: Bit iE Ei n] SERT] EA: 


rostopic pub - r 20 /Gazebo/set model state Gazebo msgs/ModelState '(model name: coke can, 
pose: ( position: ( x: 1, y: 0, z: 2 }, orientation: (x: 0, y: 0. 491983115673, z: 0, w: 
0.870604813099 ) }, twist: ( linear: ( x: 0, y: 0, z: 0 }, angular: ( x: 0, y: 0, z: 0) ), 
reference frame: world }' 
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如 图 6-14 所 示 ,用 户 应 该 看 到 可 乐 可 以 悬 停 在 RRBot 前 面 。 


图 6-14 RRBot 机 器 人 与 可 乐 眶 子 


3. Gazebo 发 布 主题 
主题 包括 : 
用 于 发 布 仿真 时 间 , 与 /use_sim_time 参数 一 起 使 用 : 


/clock: rosgraph msgs/Clock 

用 于 发 布 仿真 中 所 有 连 杆 的 状态 : 
—/link states: Gazebo msgs/LinkStates 
用 于 发 布 仿真 中 所 有 模型 的 状态 : 


~/model_states: Gazebo msgs/ModelStates 


Gazebo 发 布 /Gazebo/link states 和 /Gazebo/model_ states 主题 ,包含 关于 Gazebo 
world 框架 的 仿真 中 对 象 的 姿势 和 腕 部 信息 。 用 户 运 行 下 面 的 以 查看 动作 : 


rostopic echo —n 1 /Gazebo/model states 
或 者 

rostopic echo —n 1 /Gazebo/link states 

注意 :;“ 连 杆 ? 是 指 具 有 给 定 的 惯性 视觉 和 碰撞 特性 的 刚体 。 而 “模型 为 连 杆 和 关节 
的 集合 。“ 模 型 ”的 状态 是 其 规范 的 “ 连 杆 ”的 状态 。 鉴 于 URDF 强制 实施 树 结构 ,模型 的 规 
范 连 杆 由 其 根 链 路 定义 。 


4. 服务 : 在 模拟 环境 中 创建 和 删除 模型 
这 些 服务 允许 用 户 在 仿真 中 动态 创建 模型 : 
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用 于 使 用 此 服务 创建 通用 机 器 人 描述 格式 (URDF): 


~/spawn urdf model: Gazebo msgs/SpawnModel 


用 于 使 用 此 服务 创建 以 Gazebo 仿真 描述 格式 (SDF) 编 写 的 模型 : 


—/spawn sdf model: Gazebo msgs/SpawnModel 


用 于 此 服务 允许 用 户 从 仿真 中 删除 模型 : 

一 /delete_model : Gazebo msgs/DeleteModel 

1) 创建 模型 

我 们 提供 了 一 个 名 为 spawn_model 的 帮助 脚本 ,用 于 调用 由 Gazebo_ros 提供 的 模型 
创建 服务 。 使 用 服务 调用 方法 生成 模型 的 最 实用 的 方法 是 使 用 roslaunch 文件 。 详 细 信息 
请 查看 网 页 ( 见 http://gazebosim. org/tutorials/? tut = ros. roslaunch) 中 关于 使 用 
roslaunch 文件 到 生成 模型 部 分 内 容 。 有 很 多 方法 可 以 利用 spawn_model 向 Gazebo 添加 
URDF 和 SDF。 以 下 是 一 些 典 型 示例 。 

CD 从 文件 生成 URDF。 首 先 将 . xacro 文件 转换 为 . xml, 然 后 生成 : 


rosrun xacro xacro 'rospack find rrbot_description'/urdf/rrbot. xacro >> 'rospack find rrbot_ 
description'/urdf/rrbot. xml 


rosrun Gazebo ros spawn model — file 'rospack find rrbot description'/urdf/rrbot.xml ~ urdf 
-y1 -model rrbotl - robot namespace rrbotl 


(2) SDF 从 本 地 模型 数据 库 生 成 : 


rosrun Gazebo ros spawn model — file 'echo $ GAZEBO MODEL PATH'/coke can/model.sdf - sdf 
一 model coke canl - y 0.2 -x -0.3 


(3) SDF 从 在 线 模型 数据 库 生成 : 
rosrun Gazebo ros spawn model ~ database coke can - sdf - model coke can3 - y 2.2 -x - 0.3 


(4) 要 查看 所 有 可 用 的 参数 ,包括 spawn. model 命名 空间 ,trimesh 属性 ,关节 位 置 和 
RPY 方向 运行 : 


rosrun Gazebo ros spawn model 一 h 


2) 删除 模型 
只 需 知道 模型 名 称 就 可 删除 Gazebo 中 的 模型 。 如 果 用 户 生成 了 一 个 名 为 rrbotl 的 
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rrbot ,用 户 可 以 通过 以 下 语句 删除 : 


rosservice call Gazebo/delete model '{model_name: rrbotl]' 


5. 服务 : 状态 和 属性 设置 

这 些 服务 允许 用 户 在 仿真 中 设置 关于 仿真 和 对 象 的 状态 和 属性 信息 : 

一 /set_link_properties : Gazebo msgs/SetLinkProperties, 

一 /set_physics_properties : Gazebo_msgs/SetPhysicsProperties。 

一 /set_model_state : Gazebo_msgs/SetModelState。 

— /set model configuration: Gazebo_msgs/SetModelConfiguration 一 一 此 服务 允许 用 
户 设置 模型 关节 位 置 ,而 不 需 动 态 调用 。 

一 /set_joint_properties ; Gazebo msgs / SetJointProperties, 

—/set link state : Gazebo msgs/SetLinkState. 

—/set link state : Gazebo msgs/LinkState, 

— /set model state : Gazebo msgs/ModelState. 

设置 模型 状态 示例 如 下 。 用 /Gazebo/set_model_state 服务 在 RRBot 3T— 47 T Af fil 

如 果 用 户 还 没有 添加 可 乐 缸 到 用 户 的 仿真 ,运行 以 下 语句 : 


rosrun Gazebo ros spawn model -database coke can - Gazebo - model coke can - y 1 


这 在 Gazebo 预 包装 或 通过 在 线 模型 数据 库 。 将 可 乐 放 在 场景 中 的 任何 地 方 。 现 在 调 
用 服务 请 求 将 可 乐 移动 到 RRBot 的 位 置 ; 


rosservice call /Gazebo/set model state '{model state: { model name: coke can, pose: 
(position: ( x: 0.3, y: 0.2 ,z: 0 ), orientation: (x: 0, y: 0. 491983115673, z: 0, w: 
0.870604813099 ) ), twist: ( linear: (x: 0.0 , y: 0,z: 0 } , angular: (x: 0.0, y:0, z:0.0}}, 
reference frame: world ) )' 


用 户 会 看 到 如 图 6-15 所 示 的 场景 。 


6-15 ”将 可 乐 瓶子 移动 到 RRBot 机 器 人 旁 


使 
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用 以 下 命令 让 RPBot 旋转 : 


rosservice call /Gazebo/set model state '(model state: { model name: rrbot, pose: { position: 
{ x: 1, y: 1 ,z: 10 ), orientation: (x: 0, y: 0.491983115673, z: 0, w: 0.870604813099 } }, 
twist: ( linear: (x: 0.0, y: 0,z: 0 } , angular: (x: 0.0, y: 0, z: 0.0) } , reference frane: 
world ) } 


如 图 6-16 所 示 ,RRBot BUS A. SMATRA E — EFE BS. MAR ANI JE Pr T PLE. 


图 6-16 旋转 的 RRBot 机 器 人 


6. 服务 : 状态 和 属性 获取 
这 些 服务 允许 用 户 在 仿真 中 检索 关于 仿真 和 对 象 的 状态 和 属性 信息 : 
一 /get_model_properties: Gazebo_msgs/GetModelProperties 一 一 此 服务 返回 仿真 中 


的 模型 的 属性 。 


一 /get_model_state: Gazebo_msgs/GetModelState 一 一 此 服务 返回 仿真 中 的 模型 的 


一 /model_states: Gazebo_msgs/ModelStates 


一 /get_ world properties: Gazebo msgs/GetWorldProperties 此 服务 返回 仿真 
world 的 属性 。 

—/get joint properties; Gazebo msgs/GetJointProperties 此 服务 返回 仿真 中 关 
节 的 属性 。 

一 /get_link_properties: Gazebo msgs/GetLinkProperties 此 服务 返回 仿真 中 连 杆 
的 属性 。 

一 /get_link_state: Gazebo msgs/GetLinkState 此 服务 返回 仿真 中 连 杆 的 状态 。 

—/get physics properties; Gazebo msgs/GetPhysicsProperties 此 服务 返回 在 仿 


真 中 使 用 的 物理 引擎 的 属性 。 


一 /link_states: Gazebo msgs/LinkStates 


在 world 框架 中 发 布 完整 的 连 杆 状态 。 
在 world 框架 中 发 布 完整 的 模型 


注意 : link names 是 在 Gazebo 作用 域 的 名 称 符号 , [model name:: body name]. 
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1) 获取 模型 状态 示例 
现在 已 经 可 以 “ 跑 " 可 乐 了 , 接 下 来 用 户 会 想 知道 它 走 了 多 远 。 进 一 步 使 用 服务 调用 查 
询 踢 可 乐 时 的 姿势 和 腕 部 信 


rosservice call Gazebo/get model state '(model name: coke can]' 


这 取决 于 机 器 人 的 踢 技 能 : 


pose: 

position: 

x: —10.3172263825 

y: -1.95098702647 

z: —0.00413857755159 
orientation: 

x: — 0.0218349987011 
y: -0.00515029763403 
z: 0.545795377598 

w: 0. 83761811887 
twist: 

linear: 

x: —0.000385525262354 
y: -0.000344915539911 
z: —0.00206406538336 
angular: 

x: —0.104256200218 

y: 0.0370371098566 

z: 0. 0132837766211 
success: True 


2) 检索 仿真 世界 和 对 象 属性 
用 户 可 以 通过 运行 以 下 指令 获取 world 上 的 模型 列表 (ground_plane, coke cane. 


rrbot) : 


rosservice call Gazebo/get world properties 

sim time: 1013.366 

model names: ['ground plane', 'rrbot', 'coke can'] 
rendering enabled: True 

success: True 

status message: GetWorldProperties: got properties 


利用 以 下 命令 并 检索 特定 模型 的 详细 信息 : 
rosservice call Gazebo/get model properties '(model name: rrbot]' 


parent model name: ' 
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canonical body name: '' 

body names: ['linkl', 'link2', 'link3'] 

geom names: ['linkl geom', 'link2 geon', 'link3 geon', 'link3 geom camera link', 'link3 geom | 
hokuyo link'] 

joint names: ['fixed', 'jointl', 'joint2'] 

child model names: [] 

is static: False 

success: True 

Status message: GetModelProperties: got properties 


7. 服务 : 力 的 控制 

服务 允许 用 户 在 仿真 中 向 身体 和 关节 施加 角 动 量 和 力 : 

一 /apply_body_wrench: Gazebo_msgs/ApplyBodyWrench 一 一 在 仿真 中 将 角 动 量 应 
用 到 主体 。 所 有 应 用 于 同一 物体 的 活动 角 动 量 是 累积 的 。 

—/ apply. joint effort: Gazebo_msgs/ApplyJointEffort 一 一 在 仿真 中 对 一 个 关节 施加 
作用 力 。 对 同一 关节 应 用 的 所 有 作用 力 是 累积 的 。 

—/clear joint, forces: Gazebo_msgs/JointRequest 一 一 明确 应 用 于 一 个 关节 的 作 
用 力 。 

一 /clear_body_wrenches: Gazebo_msgs/ClearBodyWrenches 一 一 清除 应 用 角 动 量 到 
身体 。 

1) 将 角 动量 应 用 于 连 杆 

为 了 演示 在 Gazebo 体 上 的 角 动 量 应 用 ,让 我 们 生成 一 个 没有 重力 的 对 象 。 首 先 确保 
可 乐 可 以 添加 到 仿真 中 : 


rosrun Gazebo ros spawn model - database coke can - Gazebo - model coke can - y 1 


然后 发 送 服务 呼叫 : /Gazebo/set physics properties. X [4] Œ 77 « fii f [dios 42 dk (E fo] b 
受 重 力 : 


rosservice call /Gazebo/set physics properties " 

time step: 0.001 

max update rate: 1000.0 

gravity: 

x: 0.0 

y: 0.0 

z:0.0 

ode config: 
auto disable bodies: False 
sor pgs precon iters: 0 
sor pgs iters: 50 
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sor pgs w: 1.3 
sor pgs rms error tol: 0.0 
contact surface layer: 0.001 
contact max correcting vel: 100.0 
cfm: 0.0 
erp: 0.2 
max contacts: 20" 


T np iE JR ex x 方向 施加 0.01N。m 的 扭矩 ,持续 时 间 为 1 秒 , 调 用 该 /Gazebo/ 
apply body wrench 服务 ,用户 应 该 看 到 可 乐 可 以 沿 正 x 轴 旋 转 : 


rosservice call /Gazebo/apply body wrench '{body name: "coke can::link" , wrench: { torque: 
{x: 0.01, y:0, 2:0) }, start time: 10000000000, duration: 1000000000 }' 


如 图 6-17 所 示 «np SR ibn] EAE E. 


图 6-17 REFERITI RHET 


di nf RHEA AAE D D t 6 — 0. OLN * m 扭矩 持续 1 b «np R E A ME ed: 


rosservice call /Gazebo/apply body wrench '(body name: "coke can::link" , wrench: ( torque: 
(x: -0.01, y:0, z: 0} ), start time: 10000000000, duration: 1000000000 }' 


当 转 矩 的 持续 时 间 设 置 为 负 值 时 ,其 转 矩 将 无 限期 地 保持 。 要 清除 应 用 于 身体 的 任何 
活动 角 动 量 ,用 户 可 以 利用 以 下 命令 : 


rosservice call /Gazebo/clear body wrenches '(body name: "coke can::link"]"' 


2) 仿真 在 关节 中 施加 作用 力 
调用 /Gazebo/apply_joint_effort 在 关节 施加 扭矩 : 
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rosservice call /Gazebo/apply joint effort "joint name: 'joint2' 


effort: 10.0 
start time: 
secs: 0 
nsecs: 0 
duration: 
secs: 10 
nsecs: 0" 


如 图 6-18 所 示 , 连 杆 开始 旋转 。 


图 6-18 ”旋转 的 连 杆 
获取 特定 关节 的 力 ,可 调用 以 下 命令 : 


rosservice call /Gazebo/clear joint forces '(joint name: joint2)' 


8. 服务 : 仿真 控制 

以 下 服务 允许 用 户 在 仿真 中 暂停 和 开启 对 象 的 物理 学 性 质 : 

一 /pause_physics: std_srvs/Empty 一 一 暂停 物理 更 新 。 

一 /unpause_physics: std_srvs/Empty 一 一 恢复 物理 更 新 。 

一 /reset_simulation: std_srvs/Empty 一 一 重 置 包括 时 间 的 整个 仿真 。 

一 /reset_world: std_srvs/Empty 一 一 重 置 模型 的 姿势 。 

假如 用 户 想 得 到 一 张 苏 打 水 在 空中 飞舞 的 画面 截图 。 用 户 可 以 调用 暂停 物理 引擎 ; 


rosservice call Gazebo/pause physics 
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当 仿 真 暂停 时 ,仿真 时 间 停 止 ,对 象 变 为 静态 。 然 而 ,Gazebo 的 内 部 更 新 循环 (例如 自 
定义 动态 插件 更 新 ) 仍 然 在 运行 ,但 考虑 到 仿真 时 间 不 变 , 任 何 由 仿真 时 间 节 流 的 内 容 都 不 
会 更 新 。 要 恢复 仿真 ,可 以 调用 以 下 命令 : 


rosservice call Gazebo/unpause physics 


6.6.6 Gazebo 中 的 URDF 


通用 机 器 人 描述 格式 (URDF) 是 用 于 描述 机 器 人 的 所 有 元 素 的 XML 文件 格式 。 要 在 
Gazebo 中 使 用 URDF 文件 ,必须 添加 一 些 额 外 的 用 于 仿真 的 特定 标签 。 本 节 将 介绍 在 
Gazebo 中 使 用 URDF 的 必要 步骤 ,用 户 不 必 从 头 创建 单独 的 SDF 文件 或 复制 描述 格式 。 
在 hood 下 ,Gazebo 会 自动 将 URDF 转换 为 SDF. 

虽然 URDF 是 ROS 中 非常 有 用 的 标准 化 格式 ,但 是 随 着 机 器 人 技术 发 展 更 新 ,它们 缺 
少许 多 功能 。URDF 只 能 指定 单个 机 器 人 的 运动 和 动态 属性 ,并 且 不 能 在 world 中 指定 机 
器 人 本 身 的 姿态 。 它 不 能 指定 关节 环 (平行 连 杆 ) ,并 且 缺 乏 摩 擦 和 其 他 属性 ,所 以 并 不 是 通 
用 的 描述 格式 。 此 外 , 它 不 能 指定 非 机 器 人 物体 ,例如 灯光 、 高 度 图 等 。 

通过 创建 一 个 名 为 仿真 描述 格式 (Simulation Description Format,SDF) 的 新 格式 ,可 以 
解决 GazeboURDF 的 缺点 。SDF 是 从 world 级 到 机 器 人 级 的 一 切 完整 描述 。 它 是 可 扩展 
的 ,在 其 中 添加 和 修改 元 素 非常 容易 。SDF 格式 本 身 是 使 用 XML 描述 的 ,可 以 通过 简单 的 
升级 工具 ,将 旧版 本 升级 到 新 版 本 。 

1. Gazebo 概述 

让 URDF 机 器 人 在 Gazebo 中 正确 工作 需要 以 下 几 个 步骤 ,概述 如 下 。 

1) 要 求 

必须 正确 指定 并 配置 在 < inertia > 元 素 内 每 一 个 < link > 的 元 素 。 

2) 选择 
(D < Gazebo > 为 每 个 添加 一 个 元 素 < link >。 

* 将 视觉 颜色 转换 为 Gazebo 格式 

。 将 stl 文件 转换 为 dae 文件 以 获得 更 好 的 纹理 

。 添加 传感器 插件 

© < Gazebo > 为 每 个 添加 一 个 元 素 <joint >。 
。 设置 适当 的 阻尼 动力 学 参数 
。 添加 执行 器 控制 插件 
© 为 < Gazebo > 元 素 添 加 < robot > 元 素 。 
2. < Gazebo > 元 素 
< Gazebo > 元 素 的 扩展 允许 用 户 在 SDF 格式 (不 包括 URDF) 中 指定 建立 的 属性 。 可 以 
不 必 设 置 < Gazebo > 元 素 , 因 为 它 已 自动 设置 了 默认 值 。 有 3 种 不 同类 型 的 < Gazebo > 元 
素 , 分 别 用 于 < robot >,< link > 与 < joint > 的 标记 。 我 们 将 在 < Gazebo > 部 分 讨论 每 种 类 型 
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元 素 中 的 属性 。 

3. 先决 条 件 

让 用 户 的 机 器 人 在 Gazebo 中 工作 的 第 一 步 是 从 相应 的 ROS URDF 中 获得 一 个 
URDF 文件 (参照 网 页 : http://www. ros. org/ wiki/urdf/Tutorials)。 在 继续 使 用 Gazebo 
配置 机 器 人 之 前 ,通过 在 Rviz 中 查看 来 测试 URDF 。 

D 在 Rviz 中 查看 

要 检查 一 切 是 否 正常 ,在 Rviz 中 启动 RRBot: 


roslunch rrbot description rrbot_rviz. launch: 


如 果 运 行 失败 ,尝试 使 用 killall roscore 指令 或 重新 启动 RViz 以 终止 所 有 旧 roscore 进程 。 

用 户 还 能 够 在 “联合 状态 发 布 器 "窗口 中 使 用 滑 块 来 移动 两 个 关节 。 

另外 , 当 用 户 转 换 机 器 人 到 Gazebo 中 时 ,不 需 停止 Rviz 或 其 他 ROS 应 用 程序 ,这 样 用 
户 可 以 很 方便 地 在 Rviz 对 机 器 人 测试 。 

在 前 面 的 例子 中 ,Rviz 中 的 RRBot 机 器 人 从 一 个 假 的 joint_states_publisher 节点 ( 带 
有 滑 块 条 的 窗口 ) 得 到 /joint_states。Gazebo_ros_control 部 分 将 介绍 如 何 使 用 Rviz 直接 通 
过 /joint_states 监控 用 户 机 器 人 的 仿真 状态 。 

2) 检查 RRBot URDF 

以 下 指令 可 查看 rrbot. xacro 文件: 


rosed rrbot description rrbot. xacro 


注意 : 使 用 Xacro 可 以 使 一 些 连 杆 的 联合 计算 更 容易 。 还 包括 了 两 个 附加 文件 : 

。 rrbot. Gazebo 是 一 个 Gazebo 特定 的 文件 ,包括 大 多 数 Gazebo 特定 的 XML 元 素 。 
。 materials. xacro 是 一 个 简单 的 Rviz 颜色 文件 用 于 存储 rgba 值 。 

3) 在 Gazebo 中 观察 

用 户 能 够 在 Gazebo 中 启动 RRBot: 


roslaunch rrbot Gazebo rrbot world. launch 


在 启动 的 Gazebo 窗口 中 ,用 户 会 看 到 直立 的 机 器 人 。 如 图 6-19 所 示 , 尽 管 默认 情况 下 
在 物理 仿真 器 中 没有 干扰 .但 是 数字 误差 会 开始 累积 ,导致 双 倒 立 摆 在 几 秒 钟 之 后 下 降 。 

最 终 ,手臂 会 完全 停止 。 我 们 鼓励 用 户 继续 调整 和 测试 URDF 的 各 个 方面 ,以 帮助 用 
户 了 解 有 关 仿真 URDF 机 器 人 的 更 多 信息 。 

4. URDF 文件 的 头 文件 

Gazebo 中 已 经 有 很 多 API 更 改 了 URDF 格式 ,其 中 一 个 使 得 用 户 不 再 需要 Gazebo 
xml-schema 的 命名 空间 。 如 果 用 户 的 URDF 有 类 似 下 面 程序 中 的 语句 : 


606 «(| 机 器 人 仿真 与 编程 技术 


— 


图 6-19 RRBot 机 器 人 


< robot xmlns: sensor = "http: //playerstage. sourceforge. net/Gazebo/xnlschema/ # sensor" 
xmlns: controller = " http://playerstage. sourceforge. net/Gazebo/xmlschema/ 并 
controller" 
xmlns: interface = "http://playerstage. sourceforge. net/Gazebo/xnlschema/ # interface" 
xmlns:xacro = "http: //playerstage. sourceforge. net/Gazebo/xmlschema/ # xacro" 
name = "pr2" > 


用 户 可 以 删除 它们 。 根 元 素 标记 中 需要 的 所 有 内 容 都 是 机 器 人 的 名 称 , 使 用 xacro 的 
可 选 xml 命名 空间 : 


< robot name = "rrbot" xmlns:xacro = "http://www. ros. org/wiki/xacro"> 


5. < Gazebo > 元 素 标 签 
如 果 使 用 没有 reference 二 "" 属 性 的 < Gazebo > 元 素 . 则 该 元 素 可 用 于 整个 机 器 人 模型 。 
标记 < robot > 内 部 的 元 素 < Gazebo > 如 下 : 


名 称 类 型 描 3x 
static bool 如 果 设 置 为 true. 则 模型 是 不 可 移动 的 。 否 则 ,在 动态 引擎 中 仿真 模型 
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< Gazebo > 不 在 上 表 中 的 标记 内 的 元 素 直接 插入 到 生成 的 SDF < model > 标记 中 。 这 


对 插件 特别 有 用 ,如 ROS 电机 和 传感器 插件 部 分 中 所 述 。 


6. 刚性 修复 一 个 模型 到 world 


如 果 用 户 希 望 URDF 模型 永久 地 附加 到 world 框架 (地 平面 ), 则 必须 创建 一 个 
“world" 连 杆 和 一 个 连接 ,将 其 国定 到 模型 的 基 座 ,如 果 用 户 有 一 个 移动 基地 或 移动 机 器 人 ， 


则 不 需要 连 杆 或 关节 。 可 用 以 下 指令 实现 : 


<! —— Used for fixing robot to Gazebo 'base link' -一 > 


< link name = "world"/^ 


< joint name = "fixed" type = "fixed" 
< parent link = "world" /» 

«child link = "linkl"/» 

«/ joint» 


7. 连 杆 
以 下 是 来 自 RRBot 机 器 人 的 示例 连 杆 : 


<! -- Base Link --> 

< link name = "link1"> 

<collision> 

«origin xyz = "0 0 $ (height1/2)" rpy= "0 0 0"/> 
< geometry > 

Xbox size- " $ (width) $ (width) $ (heightl]"/» 
«/geonetry > 

X/collision» 


«visual? 

«origin xyz- "0 0 $ (height1/2)" rpy- "0 0 0"/> 
< geonetry > 

Xbox size- " $ (width) $ (width) $ (height1]"/» 
X/geonetry > 

< material name = "orange" /> 

</visual > 


< inertial> 

«origin xyz="0 0 1" rpy= "0 0 0"/> 
<mass value= "1"/> 

< inertia 

ixx-"1.0" ixy= "0.0" ixz= "0.0" 
iyy- "1.0" iyz- "0.0" 

izz = "1.0"/> 

</ inertial > 

</link> 


注意 : 根据 ROS REP 103 标准 测量 单位 和 坐标 约定 ,Gazebo 中 的 单位 应 以 米 和 公斤 
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为 单位 。 

1) < collision > 和 < visual > 元 素 

这 些 标签 在 Gazebo 中 和 在 Rviz 中 基本 相同 。 但 用 户 必 须 指定 < visual > 元 素 和 
< collision > 元 素 ,否则 Gazebo 不 会 调用 。Gazebo 会 将 用 户 的 连 杆 视 为 "不 可 见 ? 的 ,并 进行 
碰撞 检查 。 

为 了 提高 性 能 ,用 户 为 碰撞 几何 创建 简化 的 模型 /网 格 。 用 于 简化 网 格 的 开源 工具 是 
Blender。 另 外 也 有 许多 闭 源 工 具 可 以 简化 网 格物 体 ,例如 Maya 和 3DMax。 

标准 URDF 可 以 使 用 RRBot 中 的 标签 来 指定 颜色 : 


< material name = "orange" /> 


如 需 单 独 使 用 橙色 , 则 可 在 文件 materials. xacro 中 : 


«material name = "orange" 
<color rgba- " $ (255/255) $ (108/255) $ (10/255) 1.0"/> 
</material > 


但 这 种 指定 连 杆 颜 色 的 方法 在 Gazebo 中 不 起 作用 ,因为 它 采 用 OGRE 的 材质 脚本 来 
着 色 和 纹理 化 连 杆 。Gazebo 必须 指定 每 个 连 杆 的 材料 标签 ,例如 
< Gazebo reference = "linkl"> 


< material > Gazebo/Orange </material > 
</Gazebo > 


如 前 所 述 ,在 RRBot 示例 中 选择 了 将 所 有 Gazebo 特定 的 标记 包含 在 名 为 rrbot 
. Gazebo 的 辅助 文件 中 。 用 户 可 以 在 那里 找到 < link > 和 < material > 元 素 。Gazebo 中 的 默 
认可 用 材料 可 以 在 Gazebo 源 代码 中 的 Gazebo/media/materials/scripts/Gazebo. material 
中 找到 。 

对 于 更 高 级 或 自 定义 材质 ,用 户 可 以 创建 自己 的 OGRE 颜色 或 纹理 。 

像 在 Rviz 一 样 , Gazebo 可 以 使 用 STL 和 Collada 文件 建议 用 户 通常 使 用 Collada 
C dae) 文 件 ,因为 它们 支持 颜色 和 纹理 ,而 对 于 STL 文件 ,用 户 只 能 有 一 个 彩色 连 杆 。 

2) < inertial > 元 件 

为 了 使 Gazebo 物理 引擎 正常 工作 ,< inertial > 必须 按照 URDF 连 杆 元 素 页 (参照 网 页 : 
http://www. ros. org/ wiki/urdf/ XML /link) 上 的 文档 规定 的 内 容 提 供 元 素 。 对 于 在 
Gazebo 中 不 被 忽略 的 连 杆 ,它们 的 质量 必须 大 于 零 。 此 外 ,具有 零 主 转动 惯量 的 连 杆 可 能 
导致 在 任何 有 限 转 矩 作用 下 的 会 无 限 地 加 速 。 

为 了 在 Gazebo 中 获得 精确 的 物理 环境 ,需要 确定 每 个 连 杆 的 正确 值 。 这 可 以 通过 测 
量 机 器 人 的 零件 ,或 通过 使 用 类 似 于 Solidworks 的 CAD 软件 来 执行 。 

RRBot 机 器 人 第 一 个 连 杆 的 惯性 元 素 示例 如 下 : 
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<inertial> 

«origin xyz = "0 0 $ {height1/2}" rpy= "0 0 0"/> 
<mass value= "1"/> 

< inertia 

ixx-"1.0" ixy= "0.0" ixz="0.0" 

iyy- "1.0" iyz- "0.0" 

izz-"1.0"/» 

«/inertial > 


原始 标签 表示 此 连 杆 的 质心 。 通 过 将 质心 设置 为 RRBot 的 矩形 连 杆 的 高 度 的 一 半 ,可 
以 将 质心 定 在 中 间 。 用 户 可 以 通过 单 击 Gazebo 的 “View? 菜 单 , 并 选择 “Wireframe” 和 
“Mass of Mass” 选 项 来 检查 URDF 中 Gushbo 中 的 质心 是 否 正确 。 在 本 示例 机 器 人 中 , 质 
量 和 惯性 矩阵 都 由 值 组 成 ,因为 该 机 器 人 没有 真实 的 物理 模型 。 

3) < Gazebo > 连 杆 的 元 素 


单独 解析 的 元 素 列表 如 下 : 
名 称 类 型 描 3x 
material 值 视觉 元 素 
gravity bool 施加 重力 


链 路 速度 的 指数 速度 衰减 系数 取 值 并 将 上 一 个 链 路 速度 乘 以 (1 一 


dampingFactor | double : 
dampingFactor) 


max Vel double 最 大 接触 修正 速度 截断 项 
minDepth double | 施加 接触 校正 脉冲 之 前 的 最 小 允许 深度 
1 
- double | 由 开放 式 动力 学 引擎 (ODE) 定 义 的 接触 面 主要 接触 方向 的 摩擦 系数 jx 
fdirl string 3 元 组 指定 碰撞 局 部 参考 系 中 mul 的 方向 
ki 
» double | 用 于 刚性 体 接触 的 接触 刚度 k_p 和 阻尼 k_d 
selfCollide | bool ”| 如 果 为 true, 则 连 杆 可 能 与 模型 中 的 其 他 连 杆 发 生 冲 突 
misxConfacts | | int 两 个 实体 之 间 允 许 的 最 大 联系 人 数 。 此 值 将 覆盖 定义 的 max_contacts 元 素 


laserRetro double | 激光 传感器 返回 的 强度 值 


4) RRBot 元 件 示例 
在 RRBot 中 ,可 以 指定 两 个 非 固 定 连接 的 摩擦 系数 。 如 果 发 生 碰撞 ,可 以 更 准确 地 仿 
真 接触 的 相互 作用 力 。 以 下 是 连 杆 的 < Gazebo > 标记 示例 : 


< Gazebo reference = "link2"> 

«nul» 0.2 </mul > 

< mu2> 0.2 </mu2> 

< material > Gazebo/Black </material > 
</Gazebo > 
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8. 关节 

所 有 为 URDF 关节 记录 的 元 素 都 适用 于 Gazebo: 

* <origin >,< parent > 和 < child > 标签 是 必需 的 。 

e « calibration > 和 < safety_controller > 可 忽略 。 

* 在 < dynamics > 标记 中 ,只 有 damping 属性 可 于 Gazebo4 和 更 早 版 本 中 使 用 。 

Gazebo5 及 以 上 可 使 用 friction 属性 。 
* € limit > 标记 中 的 所 有 属性 都 是 可 选 的 。 
以 下 是 RRBot 中 使 用 的 一 个 关节 的 示例 : 


< joint name = "joint2" type = "continuous"> 

< parent link = "link2"/» 

«child link = "link3"/» 

«origin xyz = "0 $ (width) $ (height2 — axel offsetx 2)" rpy- "0 0 0"/> 
«axis xyz = "0 1 0"/» 

< dynamics damping = "0.7"/» 

«/ joint» 


通过 测试 不 同 的 阻尼 并 观察 摆动 摆 如 何 运 动 。 阻 尼 参 数 默认 值 为 0.7N * m * s/rad. 
请 用 户 调整 这 一 数值 ,以 了 解 它 对 物理 引擎 的 影响 。 
< Gazebo > 接头 的 元 素 如 下 : 


名 R 类 型 LEE: 
stopCfm " 
double | ODE 使 用 的 关节 限制 力 混合 (cfm) 和 误差 减 小 参数 (erp) 

stopErp 
provideFeedback bool 允许 接头 通过 Gazebo 插件 发 布 其 数据 ( 力 -扭矩 ) 
二 DO 如 果 此 标志 设置 为 true,ODE 将 使 用 ERP 和 CFM 来 仿真 阻尼 。 这 是 

- bool 种 比 默认 阻尼 标签 更 稳定 的 阻尼 数值 方法 。 不 推荐 使 用 
cfmDamping cfmDamping 元 素 
fudgeFactor double | 在 关节 限制 下 缩小 关节 运动 量 ,其 值 应 在 0 和 1 之 间 


9. 验证 Gazebo 模型 工具 
Gazebo 中 的 一 个 简单 的 工具 可 以 检查 用 户 的 URDF 是 否 正确 转换 为 SDF 。 运 行 : 


# Gazebo2 and below 
gzsdf print MODEL. urdf 

# Gazebo3 and above 
gz sdf — p MODEL. urdf 


这 将 显示 从 输入 URDF 生成 的 SDF 以 及 关于 生成 SDF 缺少 的 所 需 信 息 。 
注意 : Æ Gazebo 1.9 及 更 高 版 本 中 ,一 些 调试 信息 已 移 至 用 户 可 以 查看 的 日 志文 件 : 
cat ~/. Gazebo/gzsdf. log. 
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10. 在 Gazebo 中 查看 URDF 

前 文 已 经 介绍 了 如 何在 Gazebo 中 查看 RRBot。 对 于 用 户 自 定义 的 机 器 人 ,假设 其 
URDF 存在 于 /urdf 子 文件 夹 MYROBOT descriptionROS 包 中 。 

从 该 部 分 内 容 可 知 ,用 户 有 两 个 ROS 包 用 于 用 户 的 自 定义 机 器 人 : 


MYROBOT description 


MYROBOT Gazebo 


要 查看 用 户 的 机 器 人 并 在 Gazebo 中 测试 ,运行 : 


roslaunch MYROBOT Gazebo MYROBOT. launch 


这 会 启动 Gazebo 服务 器 ,用 户 的 机 器 人 的 接口 界面 客户 端 也 会 自动 生成 。 

11. 调整 用 户 的 模型 

如 果 用 户 的 机 器 人 模型 在 Gazebo 中 出 现 意外 ,可 能 因为 用 户 的 URDF 需要 进一步 调 
整 以 准确 地 在 Gazebo 中 表示 其 物理 属性 。Gazebo 中 各 种 属性 可 通过 < Gazebo > 标签 在 
URDF 中 获得 。 

12. 分 享用 户 的 机 器 人 

如 果 用 户 有 一 个 通用 的 机 器 人 ,其 他 人 可 能 也 想 在 Gazebo 中 使 用 。 用 户 可 以 添加 
URDF 到 Gazebo 模型 数据 库 , 它 是 一 个 在 线 服务 器 。 它 的 Mercurial 存储 库 位 于 
Bitbucket。 有 关 如 何 提交 请 求 以 将 机 器 人 模型 添加 到 数据 库 中 ,请 参阅 Gazebo 模型 数据 
库 文档 ( 见 以 下 网 页 : http://gazebosim. org/tutorials? tut — model. contrib&.cat = build | 


robot). 


6.7 ”实时 系统 ROS 2.0 的 介绍 


经 过 多 年 的 发 展 ,ROS1 已 经 积累 了 非常 丰富 且 稳 定 的 功能 包 集 、 各 类 工具 和 完备 的 
教程 。 

ROS 主要 具有 以 下 特点 : 

。 主要 运行 于 单个 机 器 人 上 ; 

。 拥 有 工作 站 等 级 的 计算 资源 ; 

。 没有 实时 性 的 运算 要 求 ( 实 时 性 的 运算 要 通过 特殊 的 算法 处 理 ); 

* 优良 的 网 络 连接 能 力 ; 

。 广泛 应 用 于 学 术 研究 ; 

。 具有 极 大 的 开放 性 。 
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随 着 ROS 在 科研 和 工业 的 应 用 越 来 越 广泛 .人 们 对 ROS 的 性 能 提出 了 更 多 的 需求 ,这 
些 需求 都 是 现在 的 ROS 无 法 满足 的 。 

(1) 多 机 器 人 协作 : 虽然 使 用 ROS 可 以 开发 多 机 器 人 系统 ,但 却 没有 标准 的 方法 。 这 
些 方法 都 是 基于 ROS 的 单一 主 控制 器 的 结构 上 。 

(2) 嵌入 式 平台 开发 : 目标 是 小 型 计算 机 、 单 片 机 都 能 够 直接 地 运行 ROS ,而 不 需 通 过 
设备 的 驱动 程序 运行 ROS, 

(3) 实时 系统 : 目标 是 ROS 能 够 直接 支持 实时 控制 ,而 且 一 次 控制 周期 内 的 控制 命令 
可 以 包含 进程 之 间 和 机 器 之 间 的 通信 。 

(4) 不 理想 的 网 络 状况 : 目标 是 在 网 络 连接 不 佳 的 情况 下 (如 不 良 的 无 线 网 络 信号 
灯 ),ROS 还 能 有 正常 的 表现 。 

(5) 适用 于 产品 开发 : 目标 是 基于 ROS 的 实验 室 原型 产品 能 够 转化 为 适合 在 工业 界 等 
场景 下 使 用 的 基于 ROS 的 产品 。 

(6) 构建 系统 的 规范 模式 : 目标 是 既 保存 ROS 底层 的 高 度 灵 活性 ,又 能 为 一 些 功能 (如 
生命 周期 管理 和 静态 配置 部 署 ) 提 供 清晰 的 模式 和 支持 工具 。 

为 了 满足 以 上 这 些 需求 ,ROS2 采用 了 数据 分 发 服务 (Date Distribution Service, DDS) 
通信 协议 , 它 能 够 极 大 地 提高 系统 的 实时 性 。 

图 6-20 简要 说 明了 ROSI 和 ROS2 的 系统 模型 。 在 图 6-20 的 左 侧 ,ROS1 的 实现 包括 
通信 系统 TCPROS/UDPROS。 这 种 通信 需要 一 个 主 进程 (在 分 布 式 系统 中 是 唯一 的 )。 与 
此 相反 的 是 ,ROS2 是 基于 DDS 建立 的 , 它 包 含 一 个 DDS 抽象 层 ,如 图 6-20 的 右 侧 所 示 。 
由 于 这 个 抽象 层 , 用 户 不 需要 知道 DDS API。 这 一 层 允许 ROS2 有 高 级 别 的 配置 和 优化 
DDS 的 使 用 。 此 外 ,由 于 使 用 了 DDS.ROS2 不 需要 一 个 主 进程 。 这 在 容错 能 力 方面 是 一 
个 导入 点 。 同 时 ,为 了 保持 弹性 ,OSRF 的 开发 者 们 希望 使 用 者 可 以 自己 选择 底层 所 使 用 的 
DDS 版 本 ,不 同 的 公司 会 提供 不 同 的 DDS 实现 版 本 。 这 使 得 在 分 开 的 进程 进行 通信 时 ,不 


ROSI 
应 用 层 应 用 
Client Library Client Library | 
中 同 件 层 Abstract DDS Layer 
TcPROS/UDPROS || Nodelet f 
ntra-process| 
po» API 
操作 系统 层 Linux | Linux/Windows/Mac/RTOS 


图 6-20 ROSI fil ROS2 的 结构 
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同 的 节点 可 以 使 用 不 同 的 供应 商 提 供 的 DDS。 

由 于 DDS 具有 各 种 传输 配置 (例如 截止 日 期 和 容错 能 力 ) 和 可 扩展 性 ,DDS 适合 于 实 
时 分 布 式 嵌 和 人 式 系统 。DDS 满足 分 布 式 系统 对 于 弹性 、 可 伸缩 性 、 容 错 和 安全 的 要 求 。 通 
过 减少 库 大 小 和 内 存 占用 ,DDS 可 以 为 一 些 实时 的 环境 和 一 些小 型 嵌入 式 系统 提供 解决 方 
案 。 由 不 同 的 DDS 供应 商 开 发 的 几 个 实现 已 被 应 用 于 任务 关键 型 环境 中 (例如 火车 .飞机 、 
船只 堤坝 和 金融 系统 )。 然 而 ,ROS2 必须 将 数据 转换 为 DDS 和 从 用 户 提取 DDS, 这 将 带 
来 额外 的 开销 。 

DDS 规范 描述 了 两 个 层次 的 接口 。 

COD 底层 DCPS 层 : 将 正确 的 信息 有 效 地 传递 给 真正 需要 的 接收 者 。 

(2) 可 选 的 高 层 DLRL 层 : 允许 将 服务 简单 地 集成 到 应 用 层 。 

DDS 将 分 布 式 网 络 中 传输 的 数据 定义 为 主题 ,将 数据 的 产生 和 接收 对 象 分 别 定义 为 发 
布 者 和 订阅 者 ,从 而 构成 数据 的 发 布 /订阅 传输 模型 。DDS 的 核心 是 以 数据 为 中 心 的 发 布 - 
订阅 模型 (DCPS) , 旨 在 为 分 布 式 异 构 平台 之 间 提 供 高 效 的 数据 传输 。DCPS 模型 创建 一 个 
“全 局 数据 空间 ”, 可 以 由 任何 独立 的 应 用 程序 访问 。 在 DDS 中 ,发 布 或 订阅 数据 的 每 个 进 
程 被 称 为 一 名 参与 者 ,对 应 于 ROS 中 的 一 个 节点 ,参与 者 可 以 使 用 类 型 化 的 接口 读 取 和 写 
人 到 全 局 数据 空间 。 各 个 节点 在 逻辑 上 无 主 从 关系 ,点 与 点 之 间 都 是 对 等 关系 ,通信 方式 可 
以 是 点 对 点 、 点 对 多 、 多 对 多 等 ,在 QoS 的 控制 下 建立 连接 ,自动 发 现 和 配置 网 络 参 数 。 

所 有 DCPS 实体 都 具有 QoS 策略 ,表示 其 数据 传输 行为 。QoS 的 配置 文件 定义 了 一 系 
列 方针 ,包括 耐久 性 、 可 靠 性 .队列 深度 和 采样 历史 存储 。 基 本 的 QoS 配置 文件 包括 以 下 方 
针 的 设置 。 

(1) 历史 (History) 。 

* 保留 最 后 (Keep last): 只 存储 N 个 样本 ,可 以 通过 队列 深度 选项 进行 配置 。 

。 保留 所 有 (Keep alD : 存储 所 有 的 样本 ,受到 DDS 供应 商 已 配置 资源 的 限制 。 

(2) 深度 (Depth) 。 

。 队列 大 小 : 只 和 “保留 最 后 "一 起 使 用 。 

G) 可 靠 性 (Reliability) 。 

。 最 大 努力 (Best effort): 尝试 投递 样本 ,但 当 网 络 不 稳定 时 可 能 会 丢失 。 

。 npSECReliable) ; 保证 样本 成 功 投 送 , 可 以 重 试 多 次 。 

(4) 持久 性 (Durability)。 尝 试 保持 几 个 样本 使 得 它们 可 以 被 传送 到 潜在 的 后 加 入 的 
数据 读 取 器 中 。 保 存 样本 的 数量 由 “历史 (History)” 决 定 。 这 个 选项 有 几 个 值 ,例如 
VOLATILE fl TRANSIENT LOCAL, 

在 DDS, 有 很 多 其 他 的 QoS 策略 ,ROS2 可 支持 来 扩展 其 功能 。 鉴 于 为 给 定 的 情况 选 
择 正 确 的 QoS 设置 的 复杂 性 ,ROS2 为 常见 的 使 用 情况 (例如 传感器 数据 、 实 时 等 ) 提 供 一 
组 预定 义 的 QoS 配置 文件 ,同时 为 最 常见 的 实体 提供 了 灵活 的 QoS 控制 策略 。 

由 于 DDS 提供 了 丰富 的 服务 策略 ,支持 数据 一 对 多 、 多 对 多 等 传输 模式 ,因此 采用 基于 
DDS 标准 的 中 间 件 ,可 以 大 大 简化 应 用 软件 设计 与 开发 的 工作 量 , 提 升 系统 的 设计 水 平和 
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运行 稳定 性 ,保证 数据 传输 质量 。 

下 面 通过 实验 来 进行 ROS1 和 ROS2 的 能 力 比较 。 如 图 6-21 所 示 ,在 下 列 实验 情况 下 
的 计算 ROS1 和 ROS2 的 节点 之 间 的 各 种 通信 情况 。ROS1 用 在 (1-a)、(1-b) 和 (1-c)， 
ROS? 用 在 (2-a) (2-b) 和 (2-c) 。 在 (3-a) 和 (3-b) 中 ,ROS1 和 ROS2 的 节点 共存 。 值 得 注 
意 的 是 ,由 于 (2-c) 采 用 了 内 进程 通信 , 即 共享 内 存 传输 ,因此 不 需要 使 用 DDS 技术 ,共享 内 
存 传输 用 于 (1-c) ROSI nodelet 和 (2-c)ROS2 内 进程 情况 。 在 实验 中 ,节点 之 间 的 端 到 端 
延迟 通过 发 送信 息 在 同一 台 计 算 机 上 以 便 测量 。 


(l-a): 远程 ROS1 (l-b): 本 地 ROSI (1-0): 节点 ROS1 
C^ al 
CH 


loop back 


十 


(2-a): 远程 ROS2 (2-b): 本 地 ROS2 


9 -ol € 


loop back 
sl memory 


(3-a): 远程 ROS 桥 Q-b): 本 地 ROS 桥 


局 上 -ol 
= 


图 6-21 ROSI 和 ROS2 实验 情况 


图 6-21 中 (3-a) 和 (3-b) 是 ROS1 和 ROS2 之 间 的 通信 。 由 于 ROSI 历经 多 年 的 发 展 ， 
已 经 积累 了 非常 丰富 且 稳 定 的 功能 包 集 、 各 类 工具 和 完备 的 教程 , 且 广 泛 应 用 于 各 种 机 器 人 
中 。 目 前 ROS2 支持 与 ROSI 进行 通信 。 二 者 使 用 ros_bridge 进行 通信 ,如 图 6-22 所 示 ， 
通过 动态 桥 检查 可 用 的 主题 ,得 到 主题 名 字 和 主题 类 型 来 实现 通信 。 应 该 注意 的 是 , 当 使 用 
ros bridge 进行 通信 时 只 能 使 用 最 大 努力 (Best effort) 策 略 ,因为 ros_bridge 不 支持 可 靠 
(Reliable) 策 略 。 

通过 ROS2 和 ROSI 的 实验 对 比 可 知 ,在 本 地 情况 下 ,ROS2 的 延迟 开销 并 不 小 。 延 迟 
由 两 个 数据 转换 为 DDS 和 DDS 事务 引起 。 直 到 数据 大 小 等 于 最 大 数据 包 大 小 (64KB)， 
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6-22 动态 桥 (Dynamic Bridge) 


DDS 端 到 端 延 迟 都 是 恒定 的 。 另 一 方面 , 当 大 信息 被 划分 成 多 个 数据 包 , 延 迟 大 幅 增加 。 
数据 大 小 是 否 超过 64KB 不 是 重要 的 问题 ,尤其 是 在 DDS 中 ,因为 QoS 策略 划分 数据 包 的 
管理 需要 大 量 的 处 理 时 间 API。OpenSplice 利用 多 线程 ,处 理 比 Connext 更 快 。 这 就 是 为 
什么 在 本 地 情况 底层 DDS 实现 时 使 用 OpenSplice 的 原因 。 在 远程 的 情况 下 ,延迟 开销 是 
微不足道 的 ,必须 考虑 带宽 的 吞吐 量 ,Connext 在 这 方面 优 于 OpenSplice。 这 种 恒定 吞吐 量 
的 消耗 是 可 预见 的 , 且 无 论 数据 多 么 小 都 存在 。 尤 其 是 当 多 种 类 的 主题 被 高 频 使 用 时 , 它 有 
较 大 影响 。 因 此 ,在 远程 情况 下 推荐 使 用 Connext。 

DDS 使 得 ROS2 能 够 保证 实时 性 。DDS 具有 优越 的 容错 能 力 ,因为 在 QoS 策略 下 , 它 
能 够 保存 过 去 的 数据 , 且 不 需要 一 个 主 节点 。DDS 保证 公平 的 延迟 。 此 外 ,DDS 能 够 在 多 
种 平台 运行 。 在 RTPS 协议 下 ,任何 ROS2 节点 彼此 通信 不 需 依 赖 它 的 平台 。 对 于 工人 和 式 
系统 ,目前 FastRTPS 在 线程 和 内 存 上 是 最 好 的 DDS 实现 方式 ,但 它 并 不 适合 小 型 嵌入 式 
系统 。 

在 ROS2 中 ,DDS 将 会 作为 中 间 件 ,默认 支持 转换 DDS 供应 商 提供 的 文件 。 而 且 
ROSI 中 的 . msg 文件 将 会 继续 在 ROS2 中 被 用 于 定义 界面 ,但 在 发 布 之 前 , 需 将 . msg 类 型 
的 对 象 转换 为 . idl 对 象 , 这 样 才 可 以 使 用 DDS 传输 ,如 图 6-23 所 示 。 


ROS.msg 文 件 


DDS IDL 文 件 


es 供应 商 特定 
| 使 用 1 | 代码 生成 器 


1 
ROS message ROS &DDS DDS message 
类 转换 函数 类 


— Bà] 
没有 ROS 客 户 端 库 
仍然 可 以 使 用 


由 ROS 的 客户 端 库 使 用 
图 6-23 ROS 静态 代码 生成 


另外 ,ROS2 还 可 实现 节点 生命 周期 的 管理 。 节 点 之 间 的 各 种 状态 的 相互 转换 关系 ,如 
图 6-24 所 示 。 
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图 6-24. 节点 生命 周期 管理 


ROS2 中 的 节点 有 4 个 状态 : 未 配置 的 (unconfigured)、 非 活动 的 (inactive) ,活跃 的 
(active) ,结束 的 (finalized); 有 6 个 过 渡 状 态 : 配置 中 (configuring) ,清理 中 (cleaningup)、 
关闭 中 (shuttingdown ), 激活 中 (activating )、 停 用 中 (deactivating)、 错误 处 理 中 
Cerrorprocessing) ; 它 通 过 7 个 行为 实现 管理 ,分 别 是 : 创建 (create) .配置 (configure) 、 清 
BR (cleanup) , ifti Cactive) , f£ f] Cdeactive) .关闭 (shutdown) ,Ji£ S£ (destroy) 。 

每 一 个 被 管理 的 节点 都 提供 一 个 已 知 的 界面 .根据 一 个 已 知 的 生命 周期 状态 来 执行 , 同 
时 还 可 视 为 一 个 黑箱 。 这 给 节点 开发 人 员 提 供 了 生命 周期 管理 的 功能 ,同时 方便 于 把 管理 
节点 所 创建 的 工具 应 用 于 与 之 兼容 的 节点 。 

节点 生命 周期 管理 可 以 实现 实时 代码 路 径 的 明确 分 离 ; 更 好 地 控制 ROS 网 络 ,以 确保 
正确 的 启动 顺序 以 及 节点 的 在 线 重启 动 和 转换 ; 实现 更 好 的 监督 。 生 命 周 期 可 以 加 强 所 有 
内 部 节点 同步 性 ,并 且 内 部 节点 可 以 共享 资源 。 
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ROSI 提供 了 nodelets, 它 旨 在 提供 一 种 在 单一 的 进程 中 对 一 台 机 器 运行 多 个 算法 , 且 
在 进程 内 传递 消息 而 不 会 增加 成 本 的 方法 。nodelets 允许 动态 加 载 类 到 同一 节点 中 ,尽管 
在 同一 进程 中 ,然而 它们 提供 简单 的 单独 命名 空间 ,这 使 得 nodelets 就 像 一 个 单独 的 节点 一 
样 。 值 得 注意 的 是 nodelets 有 一 些 问题 : 节点 和 nodelets 的 应 用 程序 编程 接口 是 不 同 的 ; 
创建 和 运行 nodelets 也 是 复杂 的 。 

ROS2 定义 了 非 全 局 节点 来 解决 这 些 问题 , 它 没有 跟踪 在 一 个 进程 中 的 节点 的 全 局 状 
态 , 而 是 节点 必须 注册 到 系统 来 执行 。“ 节 点 ”和 “nodelets” 之 间 没 有 区 别 ,事实 上 
“nodelets” 在 ROS2 中 不 会 成 为 一 个 概念 。 在 单一 的 进程 中 可 以 有 多 个 节点 ,它们 通过 共享 
指针 进行 高 效 的 通信 。 

另 一 个 问题 是 : 使 用 nodelets 会 很 难 去 高 效 地 使 用 一 个 共享 线程 池 , 这 导致 了 线程 池 
缺乏 ,如 图 6-25 所 示 。 


等 竺 eni 正 执行 ， 并 维持 镇定 状态 。 阻塞 
~ BD 


图 6-25 ”线程 池 缺 乏 


这 不 利于 资源 的 有 效 利用 ,拥有 很 多 的 线程 增加 了 内 存 印 记 , 降 低 了 环境 转换 的 性 能 。 
在 ROS2 中 ,为 了 解决 这 个 问题 ,引入 了 回调 组 (callback groups)。 回 调 组 是 完全 可 选择 的 
构造 ,但 在 默认 情况 下 对 用 户 隐藏 。 

可 重 入 回调 组 (Reentrant Callback Groups) 中 的 回调 必须 能 够 : 

。 作为 它们 自己 的 回调 同时 运行 ; 

。 作为 它们 组 中 的 其 他 回调 同时 运行 ; 

。 作为 其 他 组 中 的 其 他 回调 同时 运行 。 

然而 ,相互 排斥 回调 组 (Mutually Exclusive Callback Groups) 中 的 回调 : 

。 不 会 同时 运行 多 次 ; 

。 不 会 作为 它们 组 中 的 其 他 回调 同时 运行 ; 

。 但 必须 作为 其 他 组 中 的 回调 同时 运行 。 

为 了 确保 作用 在 共享 资源 上 的 回调 不 会 在 没有 用 户 锁 定 的 情况 下 同时 运行 ,用 户 可 以 
使 用 相互 排斥 回调 组 ,如 图 6-26 所 示 。 此 时 ROS2 是 足够 智能 的 ,不 会 在 同一 时 间 执 行 “ 节 
点 1 的 回调 1” 的 多 个 实例 。 这 使 得 线程 池 中 其 他 线程 处 理 能 够 处 理 其 他 回调 。 

图 6-27 和 图 6-28 分 别 展示 了 ROS 的 内 进程 和 进程 间 发 布 /订阅 模型 。ROS 支持 透明 
的 内 进程 通信 ,这 可 以 提升 节点 在 自我 通信 (发 布 /订阅 循环 ) 和 在 同一 进程 中 与 其 他 节点 通 
信 两 方面 的 性 能 。 内 进程 通信 使 用 高 效 的 共享 指针 传输 ,可 以 避免 序列 化 和 并 行 运行 ,并 且 
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避免 网 络 堆 到 和 数据 分 包 。 
准备 服务 
UR - c» r3! 
TES RR e e 可 重 入 
并 维持 锁定 状态 


图 6-26 使 用 相互 排斥 回调 组 


xt | ! [ m 
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最 初 发 布 的 数据 
6-27 内 进程 发 布 /订阅 模型 


adi 接收 
1 
序列 化 1 解 序列 
1 
1 


网 络 堆栈 网 络 堆栈 
1 


序列 化 数据 
图 6-28 进程 间 发 布 /订阅 模型 


尽管 ROSI 的 内 进程 通信 已 经 具有 不 错 的 性 能 ,但 还 是 有 隐藏 的 问题 : 在 使 用 共享 指 
针 传 输 信息 时 ,订阅 侧 的 回调 函数 实际 上 调用 了 内 进程 回调 函数 ,这 会 得 到 错误 的 结果 。 在 
ROS2 中 ,通过 使 用 所 用 权 语 义 来 跟踪 所 有 权 , 如 unique_ptr, 使 得 内 进程 通信 更 加 安全 。 
这 种 情况 下 , 当 分 配 一 个 unique ptr 指针 时 ,它们 之 间 所 有 权 是 可 以 被 跟踪 的 。ROS2 使 用 
unique ptr 指针 可 以 以 安全 的 方式 避免 重复 。 内 进程 回调 函数 在 用 户 回调 函数 之 外 来 处 
理 , 同 时 通过 内 进程 支持 更 多 的 中 间 件 QoS 策略 和 排列 行为 ,这 使 内 进程 通信 和 进程 间 通 
信 的 行为 更 一 致 。 

ROS2 seo a Popup [jest apiid iri 
这 一 实时 能 力 , 需 要 使 用 一 


应 的 操作 系统。 

(1) Linux 变 体 的 系统 或 者 专属 系统 .例如 QNX、VxWorks。 

(2) 优先 考虑 实时 线程 : 使 用 一 个 实时 时 间 表 方针 ,如 图 6-29 所 示 。 

(3) 在 实时 节点 中 避免 非 决定 论 的 来 源 ,主要 有 : 内 存 分配 和 管理 时 .在 非 实时 路 径 中 
预 分 配 资源 ; 网 络 通 路 ,特别 是 TCP/IP; 非 实时 装 管 驱动 ; 访问 硬盘 ; 页 面 错误 (锁定 地 址 
空间 , 预 错 误 堆 释 ) 等 。 

对 于 DDS, 大 多 数 开发 商 将 以 透明 方式 使 用 共享 内 存 优化 消息 流量 (甚至 进程 之 间 )， 
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TT 


实时 线程 非 实时 线程 
99 个 静态 优先 级 9 个 静态 优先 级 最 低 优先 级 
SCHED pIPO SCHED OTHER SCHED_IDLE 
SCHED RR SCHED BATCH 
SCHED_DEADLINE 
图 6-29 实时 线程 


仅仅 在 离开 本 地 主机 时 使 用 有 线 协 议和 UDP 套 接 字 , 这 大 大 提高 了 DDS 的 性 能 。ROS2 
还 允许 主题 重 分 配 (Topic Remapping) 和 别名 使 用 (TopicAliasing) 。 

对 于 主题 重 分 配 : 

同时 应 用 到 发 布 器 和 订阅 器 ; 

如 果 应 用 到 发 布 器 ,订阅 器 将 会 失去 连接 ; 

如 果 应 用 到 订阅 器 ,发 布 器 将 会 失去 连接 ,并 且 发 布 器 的 连接 将 会 被 确立 。 
对 于 主题 别名 使 用 : 

同时 应 用 到 发 布 器 和 订阅 器 ; 

。 如 果 应 用 到 发 布 器 ,连接 也 会 建立 到 订阅 器 ; 

。 如 果 应 用 到 订阅 器 ,连接 也 会 建立 到 发 布 器 。 

为 了 在 运行 期 间 实 现 主题 重 分 配 和 别名 使 用 ,发 布 器 和 订阅 器 的 节点 将 自动 创建 一 些 
ARRS: 查询 重 分 配 和 别名 ; 通过 名 字 重 分 配 主题 ; 通过 名 字 添 加 一 个 别名 ; 通过 名 字 
删除 别名 。 这 不 仅 使 得 开发 人 员 使 用 功能 更 强大 的 运行 库 工 具 成 为 可 能 ,而 且 支 持 像 多 机 
器 人 网 络 和 可 视 化 的 动态 情况 。 

ROS2 正 处 于 发 展 阶段 ,具有 很 大 的 提升 空间 。 

(1) ROS2 当前 支持 的 QoS 策略 提供 容错 能 力 不 足 以 实时 处 理 。ROS2 必须 扩大 支持 
QoS 策略 的 范围 。 

(2) 对 于 小 的 嵌入 式 系统 ,ROS2 需要 一 个 最 小 的 DDS 实现 和 最 低 抽 象 层 。 

(3) 需要 可 供 选择 的 API 来 管理 划分 数据 包 , 这 是 处 理 大 量 信息 的 关键 。 它 将 缩小 
DDS 端 到 端 延迟 ,改善 ROS2 大 量 数据 方面 的 性 能 。 

(4) DDS 支持 C,C** 和 Java 等 语言 。 然 而 ,还 没有 支持 Python 的 DDS 版 本 。 因 此 
ROS2 应 提供 一 个 最 好 的 、 功 能 完善 的 C API, Python, Rudy 和 Lisp 等 语言 可 以 通过 
CAPI 使 用 DDS。 

(5) 在 ROS2 应 用 unique ptr 实现 内 进程 通信 后 ,还 应 使 内 进程 存储 更 智能 ,允许 更 好 
的 内 存 分 配 控制 和 实时 安全 的 测试 。 

(6) 此 外 ,ROS2 还 有 更 多 值得 期 待 的 地 方 , 例 如 ,更 好 地 支持 各 种 网 络 配置 ,复杂 系统 
的 确定 性 启动 , 预 分 配 动态 信息 ,添加 或 者 删除 节点 和 主题 时 的 通知 等 。 
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本 章 小 结 


本 章 介绍 了 机 器 人 操作 系统 环境 的 搭建, 包括 如 何 安装 虚拟 机 、Linux 系统 和 ROS 系 


统 。 然 后 介绍 了 ROS 的 基础 概念 和 命令 ,程序 包 的 创建 和 编译 。 接 下 来 分 别 介绍 了 ROS 
与 MATLAB,ROS 与 V-REP. ROS 与 Gazebo 之 间 如 何 集成 的 问题 。 最 后 介绍 最 新 的 
ROS 2.0, 对 ROS 2. 0 的 原理 和 应 用 作出 了 阐述 。 
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机 器 人 操作 系统 的 应 用 


7.1 Baxter 机 器 人 与 ROS 


前 文 多 次 以 Baxter 机 器 人 为 例 ,介绍 各 种 机 器 人 仿真 的 事例 。 这 里 将 简单 地 介绍 
Baxter 机 器 人 与 ROS 之 间 的 关系 。 


7.1.1 Baxter 机 器 人 


Baxter 机 器 人 是 美国 初创 公司 Rethink Robotics 开发 的 双 臂 机 器 人 。 它 不 仅 可 以 用 于 
工业 的 作业 ,也 广泛 被 许多 高 校 用 于 科学 研究 。 

Baxter 机 器 人 最 重要 的 一 个 特点 是 安全 可 靠 ,不 会 轻易 对 周围 的 人 造成 伤害 。 这 是 因 
为 在 Baxter 机 器 人 的 内 部 结构 中 采用 了 弹性 元 件 , 从 而 减 小 了 与 外 部 环境 交互 时 可 能 产生 
的 碰撞 ,因此 Baxter 机 器 人 属于 柔性 的 机 器 人 。 它 具有 弹簧 的 弹性 特征 ,使 得 机 器 人 与 人 
或 其 他 物体 发 生 碰 撞 时 ,能 够 提供 耐 冲击 性 并 减 小 危险 。 此 外 , 它 可 以 在 有 人 类 经 过 它 旁 边 
的 时 候 减 慢 运 行 速度 ,从 而 避免 对 人 类 可 能 造成 的 伤害 。 

使 用 Rethink Robotics 提供 的 SDK ,研究 人 员 可 以 对 Baxter 机 器 人 的 功能 进行 拓展 。 
Baxter 常常 应 用 于 人 机 交互 、 人 机 协作 、 人 机 技能 传递 .末端 执行 器 的 轨迹 规划 等 方面 。 

Baxter 机 器 人 的 外 观 如 图 7-1 所 示 , 对 各 部 分 介绍 
如 下 。 

1 一 一 360 度 头 部 超声 波 雷 达 : 主要 用 于 安全 保护 ， 
可 检测 机 器 人 工作 空间 内 是 否 有 人 ,防止 发 生意 外 。 

2 一 一 头 部 摄像 头 : 机 器 人 头 部 视觉 。 

3 一 一 头 部 显示 器 : 可 显示 工作 状态 .表情 等 。 

4 一 一 7 自由 度 手 臂 : 左右 臂 都 是 7 自由 度 的 机 械 
臂 , 仿 真人 的 手臂 结构 。 

5 一 一 夹 持 器 : 位 于 手臂 末端 ,可 夹 持 物件 。 

6 一 一 手 部 摄像 头 及 距离 传感器 : 位 于 手臂 末端 , 提 图 7-1 Baxter 机 器 人 的 外 观 


622 4| 机 器 人 仿真 与 编程 技术 


供 手 部 视觉 及 手 部 与 物件 距离 等 信息 。 
7 一 一 可 移动 底座 : 可 用 于 移动 机 器 人 、 水 平 校准 等 。 


7.1.2 Baxter 机 器 人 的 控制 系统 总 体 框架 


Baxter 机 器 人 的 控制 系统 总 体 框架 如 图 7-2 所 示 ,所 有 的 计算 机 和 Baxter 机 器 人 都 必 
须 处 于 同一 局 域 网 内 。Baxter 的 服务 器 是 一 台 装 有 ROS 系统 的 电脑 , 它 可 以 跟 Baxter 机 
器 人 直接 通信 。 个 人 计算 机 则 通过 特定 格式 的 UDP(User Datagram Protocol) 数 据 包 与 
Baxter 的 服务 器 通信 ,再 由 服务 器 将 指令 传输 给 机 器 人 ,从 而 间接 控制 机 器 人 。 采 用 这 种 
控制 方案 的 好 处 在 于 : 利用 网 络 通信 的 方式 ,可 以 实现 跨 平台 控制 。 充 分 发 挥 不 同 操作 系 
统 以 及 不 同 编程 软件 的 长 处 。 


[| UDP 通信 "ni 基于 ROS 通 信 
二 


Baxter 服 务 器 


Baxter 机 器 人 
图 7-2 Baxter 机 器 人 控制 系统 总 体 框架 


服务 器 与 Baxter 机 器 人 之 间 为 基于 ROS 的 通信 ,包括 了 同步 通信 和 异步 通信 。 此 外 ， 
用 户 可 以 通过 计算 机 与 服务 器 进行 通信 ,这 时 使 用 了 UDP 通信 。UDP 是 一 种 无 连接 的 协 
议 ,运行 在 服务 器 和 客户 端的 两 个 程序 不 用 建立 连接 ,而 是 以 收发 数据 包 作 为 通信 方式 , 数 
据 包 信息 以 分 离 的 形式 传送 ,每 个 数据 包 有 独立 的 源 地 址 和 目的 地 址 。 

Baxter 机 器 人 的 核心 SDK 由 多 个 程序 包 组 成 ,具有 分 层 功 能 ,在 ROS 系统 中 处 于 
catkin 工作 区 。 用 户 能 够 使 用 Python 十 ROS 进行 编写 。 当 需要 通过 某 种 控制 算法 去 控制 
机 器 人 时 ,首先 用 户 用 Python 进行 算法 编写 (也 可 以 使 用 MATLAB/Simulink) . 然后 用 
Python 程序 进行 控制 Baxter 机 器 人 。 在 控制 模式 上 ,Baxter 机 器 人 提供 了 3 种 控制 模式 ， 
用 户 可 以 在 顶层 进行 选择 。 这 3 种 控制 模式 分 别 是 : 

(1) 位 置 模式 : 通过 给 定 关节 的 期 望 位 置 ,Baxter 机 器 人 通过 运行 自 带 的 控制 器 ,达到 
期 望 位 置 。 

(2) 速度 模式 : 通过 给 定 各 个 关节 的 期 望 速度 ,Baxter 机 器 人 的 各 个 关节 便 会 以 期 望 
速度 进行 相应 的 运动 。 

G) 力矩 模式 : 通过 给 定 各 个 关节 的 力矩 ,Baxter 机 器 人 的 各 个 关节 在 对 应 力矩 的 作 
下 而 做 出 相应 的 运动 。 


7.1.3 相关 的 ROS 代码 


在 接 下 来 的 3 个 ROS 应 用 例子 中 ,我 们 将 通过 按照 Baxter 机 器 人 的 控制 框架 ,对 机 器 
人 进行 遥 操 作 。 实 验 中 ,使 用 一 台 计 算 机 通过 UDP 与 Baxter 机 器 人 服务 器 进行 通信 ,然后 
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通过 ROS 对 Baxter 进行 控制 。 对 Baxter 进行 直接 控制 的 Python 程序 , 它 既 可 以 实现 对 
Baxter 机 器 人 的 遥 操 作 , 也 可 以 实现 机 器 人 在 运动 过 程 中 实现 避 障 的 效果 。 
由 于 Python 的 程序 过 长 ,我 们 将 只 介绍 重要 的 代码 模块 。 


7.2 基于 神经 网 络 实现 对 摇 操 作 机 器 人 进行 高 性 能 控制 


下 面 介绍 基于 神经 网 络 实现 对 摇 操 作 机 器 人 进行 高 性 能 控制 的 系统 架构 、 编 程 与 实现 。 

在 过 去 十 年 中 , 遥 操 作 机 器 人 已 经 广泛 应 用 于 人 类 无 法 接近 的 工作 场合 ,例如 处 理 放射 
人 性 物质 和 在 危险 环境 下 的 搜索 工作 。 与 在 结构 确定 性 环境 下 工作 的 机 械 手 相 比 , 遥 操作 机 
器 人 可 以 在 动态 仿 人 环境 下 完成 更 多 样 化 的 操作 任务 。 在 大 多 数 远程 操作 系统 中 ,环境 信 
息 应 该 直接 反馈 给 操作 者 ,因此 ,操作 者 每 次 可 以 与 机 器 人 进行 有 效 交 互 。 当 环境 比较 稳定 
时 ,操作 者 可 以 通过 这 种 方式 轻易 地 操纵 远程 机 器 人 ,但 是 在 动态 和 不 确定 的 环境 中 ,这 种 
方法 可 能 对 操作 人 员 带 来 极 大 的 工作 负担 。 通 常 , 远 程 机 器 人 系统 受到 两 种 不 确定 性 的 影 
响 : 一 个 是 机 器 人 所 处 的 外 部 环境 ,例如 潜在 的 障碍 物 ; 另 一 个 则 是 机 器 人 内 部 的 不 确定 
性 ,如 未 知 动力 学 关系 和 变化 的 负荷 。 对 于 外 部 不 确定 性 ,部 分 反馈 给 操作 者 的 环境 信息 可 
能 限制 远程 操作 系统 的 应 用 范围 ,而 全 部 反馈 可 能 会 分 散 操作 者 对 工作 任务 的 注意 力 。 

在 共享 控制 框架 中 ,远程 机 器 人 的 部 分 控制 工作 是 自动 完成 的 ,可 以 辅助 人 类 操作 者 的 
神经 运动 控制 。 为 了 减少 操作 员 的 工作 量 , 考 虑 采用 共享 控制 框架 ,并 将 自动 避免 碰撞 机 制 
嵌入 到 远程 操作 系统 中 ,以 使 远程 机 器 人 能 够 安全 地 与 动态 环境 进行 交互 。 在 这 种 控制 策 
咯 下 ,操作 者 将 能 够 集中 精力 于 远程 机 器 人 末端 执行 器 的 操纵 ,并且 通过 使 用 元 余 机 制 可 以 
自动 避 障 ,而 且 对 末端 执行 器 只 有 很 小 的 影响 。 在 以 前 关于 机 械 臂 避 障 的 研究 中 ,通常 使 用 
机 械 臂 的 宛 余 自 由 度 。 在 基 座 附近 ,剩余 的 自由 度 可 能 不 足以 使 机 械 臂 沿 与 障碍 物 的 运动 
相反 的 方向 移动 。 因 此 ,机 械 臂 可 能 遇 到 无 法 解决 的 动力 学 的 问题 。 因 此 我 们 开发 了 一 种 
降 维 方法 来 解决 这 个 问题 .以便 更 好 地 利用 元 余 机 制 。 当 障碍 物 远 离 机械 辟 时 ,将 机 械 臂 恢 
复 到 自然 姿势 。 为 此 ,我 们 基于 运动 学 的 远程 机 器 人 引入 并 行 仿真 系统 。 所 提出 的 方法 可 
以 从 运动 学 角度 出 发 ,在 具有 动态 障碍 的 不 确定 环境 中 为 机 器 人 提供 性 能 保证 的 遥 操 作 。 

内 部 不 确定 性 主要 受 机 器 人 动力 学 模型 的 精度 影响 。 机 器 人 机 械 臂 的 动态 控制 方法 可 
以 分 为 无 模型 控制 和 基于 模型 的 控制 。 大 多 数 无 模型 的 方法 可 能 不 会 产生 良好 的 瞬 态 响 
应 ,而 基于 模型 的 控制 方法 通常 产生 更 好 的 控制 性 能 。 事 实 上 ,控制 性 能 在 很 大 程度 上 取决 
于 模型 精度 ,但 是 机 器 人 精确 的 动力 学 模型 永远 不 可 能 预先 获得 。 此 外 ,未 知 或 变化 的 载荷 
使 得 我 们 无 法 预先 获得 准确 的 动态 模型 。 为 了 解决 这 些 问题 ,研究 者 们 已 经 开发 了 基于 近 
似 的 控制 方法 ,并 且 已 经 成 功 地 应 用 于 实际 系统 ,例如 编队 控制 .多 智能 体 共 同 控制 和 机 器 
人 机 械 臂 控制 。 这 些 近 似 的 控制 方法 的 理论 基础 是 , 当 系 统 动力 学 满足 某 些 条 件 时 ,不 确定 
的 非 线 性 可 以 通过 诸如 神经 网 络 多项式 方 法 .小波 网 络 和 模糊 逻辑 系统 的 工具 逼近 。 

本 节 将 自 适应 神经 控制 与 误差 变换 技术 相 结 合 , 以 在 运动 学 层面 实现 可 靠 的 跟踪 性 能 。 
在 动力 学 水 平 上 ,确保 可 以 避 开 障碍 .并 且 在 动态 水 平 上 实现 基于 神经 网 络 的 控制 器 设计 与 
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运动 学 水 平 的 控制 器 设计 无 颖 集成 。 

所 设计 的 系统 框架 图 如 图 7-3 所 示 。 在 运动 学 水 平 上 是 在 关节 空间 中 生成 用 于 机 械 臂 
的 末端 夹 持 器 的 参考 轨迹 ,以 跟随 操作 者 的 指令 同时 实现 避 障 功能 ; 在 动力 学 上 是 在 存在 
不 确定 性 条 件 下 可 以 确保 参考 轨迹 被 跟踪 并 满足 特定 的 性 能 要 求 。 


T 运动 学 层次 e 动力 学 层次 3 
go. 58K 5 a poeuccuus UMEN c oic ue 
Re ton 1 
任务 空间 检测 器 | i 
O mit L 


4 关节 位 置 


控制 器 


+ 关节 空间 
- 3 x, [Urna 


图 7-3 系统 框架 图 


7.2.1 控制 系统 的 架构 


1. 系统 组 成 

如 图 7-4 所 示 为 实验 组 件 图 ,操作 人 员 通 过 使 用 连接 到 主 计算 机 的 全 方位 操纵 杆 , 采 集 
操纵 杆 轨迹 并 发 送 到 机 器 人 服务 器 以 远程 操作 机 器 人 手臂 。Baxter 机 器 人 的 其 中 一 个 机 
械 臂 被 用 作 机 械 臂 。 它 的 全 部 7 个 关节 都 运用 到 了 实验 中 。 连 接 到 下 位 机 的 机 器 人 机 械 臂 
tj Kinect 传感器 一 起 工作 ,以 检测 周围 环境 中 的 障碍 物 。 


i 
Cy 


操作 者 


图 7-4 实验 组 件 图 


Kinect 传感器 是 由 Microsoft 开发 的 基于 红 - 
色 绿色、 蓝 色 加 深度 (RGB-D) 的 图 像 传感器 ,如 
图 7-5Cb) 所 示 , 它 包含 RGB 相机 和 深度 传感器 。 r 
利用 RGB 彩色 图 像 以 及 深度 图 像 ,能 够 生成 一 

个 彩色 的 3D 点 云 ,可 以 使 用 Kinect 传感器 来 感 9 SACRA — 


知 远程 机 器 人 的 周围 环境 。 图 7-5 表 控 操作 系统 中 使 用 的 设备 


第 7 章 ”机 器 人 操作 系统 的 应 用 | 625 


六 自由 度 SensAble 多 方位 操纵 杆 , 如 图 7-5(a) 所 示 。 前 三 个 关节 决定 了 末端 执行 器 的 


第 卡 儿 空 间 位 置 ,后 三 个 关节 决定 它 在 笛 卡 儿 空 间 中 的 方向 。 
2. 工作 区 间 匹 配 
为 了 充分 利用 机 械 手 工作 区 间 , 有 必要 建立 机 器 人 和 多 方位 操纵 杆 相互 匹配 的 工作 


区 


间 , 以 使 多 方位 操纵 杆 的 缩放 工作 区 间 尽 可 能 与 远程 机 器 人 操作 器 的 工作 空间 重 释 。 输 入 


设备 和 机 械 辟 的 末端 执行 点 的 点 云 是 基于 蒙特 卡 罗 方 法 创建 的 。 


cosg 一 sin8 0 
Xa = |sing cosh 0|X (Smxm + Tu) 
0 0 1 


其 中 ,xs 一 [ze ya zal” 指 的 是 机 械 臂 末端 执 行 器 的 笛 卡 儿 位 置 ,单位 是 米 ; xm 一 [zw 


2,3) 


Yn m] 指 的 是 多 方位 操纵 杆 尖端 的 笛 卡 儿 位 置 ,单位 是 毫米 。 旋 转角 p= 下 是 围绕 机 器 
人 机 械 手 基本 框架 的 Z 轴 旋转 。S， 二 diag{0.0041,0.0040,0. 0041} 是 选择 好 的 缩放 因子 ， 


而 T, —[0. 701,0. 210,0. 129 ]" 则 为 偏 移 量 。 

3. 坐标 变换 

如 图 7-6 所 示 ,Kinect 传感器 被 用 来 检 
测 机 器 人 机 械 臂 周围 的 障碍 物 。 为 了 实现 障 
碍 物 的 检测 和 避 障 ,在 机 器 人 机 械 臂 和 
Kinect 的 坐标 系 之 间 建 立 变换 矩阵 工 是 必要 
的 。T 和 矩阵 可 以 通过 校准 方法 获得 。 

首先 ,我 们 考虑 4 个 非 共 线 点 ,并 在 机 器 
人 坐标 系 和 Kinect 传感器 坐标 系 下 测量 它们 
的 坐标 。 指 定 XYZ 为 机 器 人 的 坐标 系 ,而 六” 
Y'Z' Jy Kinect 的 坐标 系 。 指 定 XYZ 坐标 系 
下 4 个 非 共 线 点 的 坐标 为 (zi1,y1,z1), Caz, 
yayzz),(zsyysyzs), (ziy yas zi s 而 在 XY 
Z' 坐 标 系 下 的 坐标 为 (Xi ,yi zi rz y2 
可 以 计算 出 转换 矩阵 工 


7-6 Kinect 传感器 的 设置 


, 
»z2) 


17-1 
T2 T3 T4 


X»: e ys » 


Ei z 
y » 1 i 1 :] 1 1 
这 两 个 坐标 系 之 间 的 转换 可 以 通过 该 式 子 获 得 


wei y e 


xs 


Lx » x 
4. 碰撞 点 的 识别 


, (zs,yi,z3) (zi,y4zi)。 基 于 这 两 个 坐标 系 


(7.2) 


首先 ,连续 & 均 值 聚 类 方法 用 于 将 从 Kinect 传感器 获得 的 点 云 分 割 成 超 像 素 。 机 器 人 
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上 的 超 像素 将 通过 其 运动 学 构建 的 机 器 人 骨架 模型 来 识别 。 每 个 机 器 人 机 械 臂 的 连接 可 以 
被 视 为 3D 空间 中 的 一 条 线段 ,如 图 7-7 所 示 。 基 于 正 向 运动 学 ,可 以 以 下 列 方式 计算 每 个 
关节 的 坐标 , 即 每 个 节 段 的 端点 的 笛 卡 儿 位 置 : 

X, =°A AsSITnglAUX; (0.3) 
Hp X, = ss" yosz,41]7 A Xi Dr yism 117 J&fü -FL2S A p 03898 f EAE. E 
We! A; 是 它们 之 间 的 齐 次 变换 矩阵 。 基 于 分 段 点 云 可 以 轻松 地 实时 生成 机 器 人 的 3D 模 
型 , 即 图 7-8(b) 中 的 球体 组 成 的 红色 3D 模型 。 在 点 云 中 ,3D 模型 周围 的 点 可 以 视 为 障碍 
物 。 碰 撞 点 pv 和 ze 如 图 7-7 所 示 。 这 两 个 点 中 ,前 者 在 机 器 人 上 ,后 者 在 障碍 物 上 ,机 器 
人 机 械 辟 和 障碍 物 之 间 的 最 短 距离 为 4。 图 7-8 中 直接 提供 检测 结果 ,其 中 图 7-8(b) 中 的 
红色 3D 模型 是 机 器 人 的 3D 模型 ,点 云 中 的 绿色 点 ( 见 图 7-8(a) 左 侧 ) 表 示 障 碍 物 , 蓝 色 和 
红色 点 分 别 表示 左 臂 和 右 臂 上 的 碰撞 点 , 黑 线段 ( 见 图 7-8(a)、(c)) 表 示 机 器 人 机 械 臂 和 障 
碍 物 之 间 的 距离 。 


图 7-8 障碍 物 检测 示意 图 


避 障 决策 示意 图 见 图 7-9. WEISSI Mii, 落 在 ,的 法 向 平面 N。 上 。 

并 行 仿真 系统 见 图 7-10。 实 际 机 械 臂 的 骨架 由 实心 黑 线 表 示 ,虚线 黑 线 表示 在 并 行 系 
统 中 仿真 的 人 工 机 械 臂 。 

未 知 系统 动力 学 估计 实验 见 图 7-11。 
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(a) 决策 出 来 的 瞬时 避让 速度 zw (b) 可 选 的 避免 速度 io 
图 7-9 避 障 决策 示意 图 


图 7-10 ”并行 仿真 系统 图 7-11 未 知 系统 动力 学 估计 实验 


7.2.2 实验 设计 与 实现 

本 书 中 的 神经 网 络 的 实现 是 通过 Simulink 实现 的 ,下 面 列举 实验 中 重要 的 Simulink 模块 。 
1. 输入 模块 

图 7-12 为 输入 模块 Simulink 框图 。 


图 7-12 输入 模块 Simulink 框图 
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Decoderl 的 相关 代码 为 : 


function [left lamda, left qd, left dad, left q, left dg, right lamda, right qd, right dad, 
right q, right dq] 7 fcn(data, N) 
persistent Data; 
if isempty(Data) 
Data = single(data); 
end 
left lamda = [000000 0]*; 
left ad = [000000 0]'; 
left dad = [000000 0]'; 
left q = [000000 0]'; 
left dq = [000000 0]'; 
right landa = [000000 0]'; 
right qd = [0000 00 0]' 
right dad = [0000 0 0 0] 
right q = [000000 0]'; 


right dq = [00000 00]'; 
ifN 一 = 0 

Data = single(data); 
end 
fori - 0:6 


left lamda(i+1) = (Data(1*2* i)+Data(2+ 2» i) *256)/1000.0; 

left qd(i+1) = (Data(15 +2» i) + Data(16 *2*i)*256 — 32768)/10000.0; 
left dqd(i-1) = (Data(29 +2 i) -*Data(30*2*i)*256 - 32768)/10000.0; 
left q(i*1) = (Data(43 +2 » i) + Data(44 + 2 i) * 256 — 32768)/10000.0; 
left dq(i+1) = (Data(57 *2* i) + Data(58 +2» i) *256 — 32768)/10000.0; 


right lamda(i+1) = (Data(71-4 2 i) * Data(72 * 2 x i) « 256)/1000.0; 

right qd(i*1) = (Data(85 +2» i) + Data(86 + 2 x i) 256 - 32768)/10000.0; 

right dqd(i+1) = (Data(99 + 2x i) + Data(100 + 2» i) «256 - 32768)/10000.0; 

right q(i*1) = (Data(113*2» i)  Data(114 +2 x i) «256 — 32768)/10000.0; 

right da(i+1) = (Data(127 + 2 » i) + Data(128 42» i) + 256 — 32768)/10000.0; 
end 


2. 数据 处 理 模块 
图 7-13 为 数据 处 理 模块 Simulink 框图 。 


图 7-13 数据 处 理 模块 Simulink 框图 
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其 中 ,matricsZ_g 的 代码 为 : 


3. 输出 模块 
图 7-14 为 输出 模块 Simulink 框图 。 
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Encoder Send 
图 7-14 输出 模块 Simulink 框图 
Encoder 模块 代码 如 下 : 


function data = fcn(taul left, taul right) 
data = uint8([0000000000000000000000000000]); 


dataraw = taul left; 

dataraw = uintl6(dataraw * 1000 + 32768) ; 

data(1) = uint8(bitand(dataraw(1), hex2dec( 'FF00') )/256) ; 
data(2) = uint8(dataraw(1) — uintlé(data(1)) * 256); 
data(3) = uint8(bitand(dataraw(2), hex2dec( 'FF00') )/256) ; 
data(4) = uint8(dataraw(2) - uintl6(data(3)) * 256); 
data(5) = uint8(bitand(dataraw(3), hex2dec( 'FF00') )/256) ; 
data(6) = uint8(dataraw(3) - uintlé(data(5)) * 256); 


data(7) = uint8(bitand(dataraw(4), hex2dec( 'FF00') )/256) ; 
data(8) uint8(dataraw(4) — uintlé(data(7)) * 256); 
data(9) = uint8(bitand(dataraw(5), hex2dec( 'FF00') )/256) ; 
data(10) = uint8(dataraw(5) - uintl6(data(9)) x 256); 
data(11) = uint8(bitand(dataraw(6), hex2dec( 'FF00'))/256) ; 
data(12) = uint8(dataraw(6) - uintl6(data(11)) * 256); 
data(13) = uint8(bitand(dataraw(7), hex2dec( 'FF00'))/256) ; 
data(14) = uint8(dataraw(7) - uintl6(data(13)) * 256); 


dataraw taul right; 

dataraw uintl6(dataraw * 1000 * 32768) ; 

data(15) = uint8(bitand(dataraw(1), hex2dec( 'FF00'))/256) ; 
data(16) = uint8(dataraw(1) - uintl6(data(15)) * 256); 
data(17) = uint8(bitand(dataraw(2), hex2dec( 'FF00'))/256) ; 
data(18) = uint8(dataraw(2) - uintl6(data(17)) * 256); 
data(19) = uint8(bitand(dataraw(3), hex2dec( 'FF00'))/256) ; 
data(20) = uint8(dataraw(3) - uintl6(data(19)) * 256); 
data(21) = uint8(bitand(dataraw(4), hex2dec( 'FF00'))/256) ; 
data(22) = uint8(dataraw(4) - uintl6(data(21)) * 256); 
data(23) = uint8(bitand(dataraw(5), hex2dec( 'FF00'))/256) ; 
data(24) = uint8(dataraw(5) - uintl6(data(23)) * 256); 
data(25) = uint8(bitand(dataraw(6), hex2dec( 'FF00'))/256) ; 
data(26) = uint8(dataraw(6) - uintl6(data(25)) * 256); 
data(27) = uint8(bitand(dataraw(7), hex2dec( 'FF00'))/256) ; 
data(28) = uint8(dataraw(7) - uintl6(data(27)) * 256); 
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7.2.3. 实验 及 结果 


1. 神经 网 络 学 习 的 性 能 测试 
第 一 组 实验 主要 测试 由 未 知 动力 学 和 不 确定 载荷 引起 的 效应 的 补偿 效果 。 如 图 7-11 
所 示 ,Baxter 右 臂 的 末端 执行 器 被 控制 沿 着 固定 的 轨迹 移动 。 轨 迹 定 义 如 下 : 
0.6 十 0. 1sin(2rt/2.5) 
xa) = | 一 0.4 十 0.3cos(2rt/2.5) (7.4) 
0.2 
载荷 是 施加 在 夹具 上 的 ,夹具 具有 1. 3kg 的 重量 。 为 了 高 精度 实现 7- 自 由 度 下 机 器 人 
动力 学 的 近似 ,我 们 对 于 每 个 输入 维度 选择 3 个 节点 ,并 且 采 用 53 个 节点 的 神经 网 络 


MC) 二 WHIZw(0) 和 CG(0) 二 WEZe(0) ,或 21 个 NN 节点 的 神经 网 络 C(0) 二 WEZc(0)" 和 41 个 
NN 35 PL Bh ES FE 4 f. WIZ, (0.0 s vasba)" NN 权重 矩阵 的 初始 状态 为 Ww (0) —0€ 


RY” ,We(0) -0€ R^" .Wc(0) =0E E" IW, (0) —0€ R*"*" 3E m ,1—2187 ,n— 7, 

本 节 将 进行 两 个 比较 实验 以 验证 我 们 提出 的 基于 神经 学 习 的 控制 器 的 性 能 。 在 第 一 个 
实验 中 , 带 有 负载 的 机 械 手 采用 神经 学 习 的 控制 器 进行 控制 ,而 在 第 二 个 实验 中 ,采用 了 我 
们 提出 的 神经 学 习 方 法 。 

对 于 第 一 个 实验 ,参考 关节 角度 @ 和 实际 关节 角度 8 如 图 7-15 所 示 ; 关节 角度 误差 es 
如 图 7-16 所 示 。 我 们 看 到 : 由 于 负载 太 大 ,关节 角度 误差 相对 较 高 。 


erad) 
h 


[VV VVVVVVVVVVVVVNYV 
0 5 10 15 20 25 30 35 40 45 
tls 


图 7-15 没有 神经 网 络 学 习 控 制 的 参考 关节 角度 & 和 没有 神经 学 习 的 实际 关节 角度 @ 
ik. 虚线 和 实 线 分 别 表示 参考 和 实际 关节 角度 。 不 同 颜色 的 线 表示 不 同 的 关节 -。 


提出 的 神经 网 络 学 习 方法 的 实验 结果 如 图 7-14 一 图 7-19, 图 7-17 显示 的 是 补偿 转移 
Txw 二 M54 十 Cvs 十 G。 当 负载 附 接 到 端 部 执行 器 时 ,其 重力 引起 的 不 确定 性 比 机 器 人 机 械 


手 的 动态 不 确定 性 更 明显 。 所 以 在 图 7-20 中 特意 展示 了 NN 权重 矩阵 Wo, 而 图 7-18 显示 
了 参考 和 实际 关节 角度 ,图 7-19 展示 了 关节 的 角度 误差 。 可 以 看 到 ,在 开始 时 ,补偿 扭矩 为 
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图 7-18 神经 网 络 学 习 方 法 下 的 参考 关节 角度 负 和 实际 关节 角度 @ (虚线 和 实 线 分 别 表示 参考 和 
实际 关节 角度 。 不 同 颜色 的 线 表示 不 同 的 关节 ) 
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零 ,关节 角度 误差 与 图 7-16 所 示 的 结果 相同 。 后 来 权重 矩阵 显示 了 收敛 的 趋势 。 在 补偿 扭 
A. Tww 中 可 以 找到 与 参考 轨迹 相同 的 周期 性 的 变化 。 随 着 补偿 扭矩 的 增加 ,关节 角 误 差 迅 
速 减 小 ,并 在 几 个 周期 后 得 到 足够 小 的 效果 ,如 图 7-19 所 示 。 


图 7-19 神经 网 络 学 习 的 关节 角 误差 ee( 不 同 灰 度 的 线 表示 不 同 的 关节 ) 


lwall 


图 7-20 NN 权重 矩 阵 We 的 每 个 列 向 量 的 范 数 ,对 应 矩阵 G 的 每 一 行 


2. 跟踪 性 能 测试 

第 二 组 实验 主要 集中 在 动态 控制 器 的 跟踪 性 能 的 测试 。 控 制 机 械 手 的 末端 执行 器 沿 着 
两 个 点 Pi: (0.6, 一 0.2.0.2) 和 P2: (0.6, 一 0.6.0.2) 之 间 的 直线 移动 。 进 行 两 个 比较 实 
验 : 采用 误差 变换 方法 和 没有 采用 误差 变换 方法 。 没 有 采用 误差 变换 方法 的 结果 显示 在 
图 7-21(a)、(b)。 而 采用 我 们 提出 的 误差 变换 方法 的 结果 如 图 7-21(c)、(d) 所 示 。 图 中 显 
示 的 是 其 中 一 个 关节 的 实验 ,其 他 关节 的 实验 结果 类 似 。 为 了 比较 这 两 个 实验 之 间 的 性 能 ， 
在 图 7-21(b)、(d) 画 出 它们 的 性 能 函数 p(1)。 在 没有 误差 变换 的 情况 下 , 超 调 要 大 得 多 ,并 
且 稳 定时 间 比 使 用 了 误差 变换 的 方法 长 得 多 。 值 得 注意 的 是 ,我 们 提出 的 方法 的 关节 角度 
误差 在 瞬 态 过 程 中 从 未 超出 性 能 要 求 , 如 图 7-21(d) 所 示 。 这 意味 着 瞬 态 性 能 满足 要 求 。 
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tls tls 
(c) 采用 误差 变换 法 的 角度 (d) 采用 误差 变换 法 的 误差 
图 7-21 跟踪 性 能 测试 的 实验 结果 


3. 避 障 的 测试 

在 该 实验 组 中 ,我 们 测试 了 避 障 性 能 。 操 作者 控制 机 械 手 在 两 个 固定 位 置 A 和 B 之 间 
移动 物体 ,如 图 7-22 中 的 桌子 上 的 两 处 十 字 
叉 , 绿 色 盒 子 是 操作 对 象 。 根 据 是 否 采用 我 nm 
们 提出 的 避 障 方法 ,进行 了 两 个 实验 。 障碍 

实验 结果 如 图 7-23 和 图 7-24 所 示 。 我 
们 看 到 , 当 机 器 人 的 肘 部 靠近 障碍 物 时 ,在 没 
有 如 障 控制 策略 下 机 械 手 与 障碍 物 发 生 碰 
撞 , 不 能 完成 任务 。 相 比 之 下 ,在 我 们 设计 的 
避 障 控制 模式 下 ,机械 手 可 以 调整 其 姿势 以 
避 开 障碍 物 并 且 能 够 完成 任务 。 


Za p 14 


图 7-22 ”和 避 障 实验 实物 图 


(a) 不 避免 碰撞 (b) 避免 碰撞 
图 7-23 ” 避 障 实验 的 视频 截图 


第 7 章 ” 机 器 人 操作 系统 的 应 用 P 635 


Zm) 


一 banans 
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(a) 避 障 失败 (b) 避 障 成 功 


图 7-24 机 械 手 姿势 的 时 间 序 列 ( 黑 线 表示 机 械 手 , 蓝 色 长 方 体 表示 障碍 物 ) 


4. 姿态 恢复 性 能 测试 

最 后 的 实验 是 测试 恢复 功能 。 为 了 便于 测试 ,操作 者 静止 地 握 住 操纵 杆 , 使 得 机 械 手 的 
末端 执行 器 保持 在 固定 位 置 ,同时 障碍 物 以 动态 方式 移动 ,测试 结果 如 图 7-25 所 示 。 在 图 
中 可 以 看 到 , 当 障 碍 物 靠近 机 械 手 移动 时 ,其 肘 部 向 下 移动 以 避免 可 能 存在 的 碰撞 。 注 意 
到 ,在 这 个 过 程 中 ,末端 执行 器 几乎 没有 变化 位 置 ; 当 障 碍 物 移 开 时 ,机 械 臂 自动 恢复 到 其 
原来 的 姿势 。 


(c) (7395 (d) 7743s 


图 7-25 姿态 恢复 实验 结果 
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7.3 规定 全 局 稳定 性 和 运动 精度 的 双 臂 机 器 人 的 神经 网 络 
控制 


本 节 介 绍 规定 全 局 稳定 性 和 和 运动 精度 的 双 臂 机 器 人 的 神经 网 络 控制 的 原理 、 编 程 与 

人 类 通过 双手 协作 能 够 执行 精细 和 复杂 的 操作 , 双 臂 协作 是 机 器 人 领域 的 研究 焦点 之 
一 。 与 单 臂 机 器 人 相 比 , 双 臂 机 器 人 在 操作 和 装载 方面 具有 显著 的 优点 。 例 如 ,在 雕刻 或 螺 
纹 连接 的 任务 的 工具 中 ,与 单个 机 器 人 臂 相 比 ,要 将 完成 任务 所 需 的 运动 和 力 的 分 配给 两 个 
机 器 臂 上 ,这样 大 大 降低 了 操作 的 复杂 性 和 能 量 成 本 。 概 述 图 见 图 7-26. 


图 7-26 双 臂 机 器 人 机 械 手 操作 同一 个 对 象 的 概述 图 


应 当 强调 的 是 ,运动 精度 对 于 双 臂 操作 是 非常 重要 的 。 两 臂 的 精确 配合 可 以 确保 不 会 
发 生 过 大 的 内 力 ,并 且 还 减少 内 力 的 可 能 变化 。 双 臂 机 器 人 控制 器 框图 见 图 7-27。 在 这 方 
面 ,对 运动 精度 的 严格 要 求 意味 着 还 必须 考虑 操作 中 的 瞬时 性 能 。 因 此 ,研究 者 们 在 控制 领 
域 已 经 做 出 了 很 多 努力 来 实现 期 望 的 瞬 态 性 能 。 


XU Las A. 
神经 网 络 控制 器 
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图 7-27. 双 辟 机 器 人 控制 器 框图 
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在 实践 中 ,机 器 人 的 运动 学 信息 可 以 从 制造 商 那里 准确 地 获知 ,而 动力 学 信息 往往 具有 
较 大 的 不 确定 性 。 然 而 ,我 们 可 以 利用 机 器 人 系统 的 输入 -输出 数据 ,并 用 它们 来 逼近 机 器 
人 动力 学 参数 ,以 便 设 计 出 具有 令 人 满意 的 控制 器 。 最 成 功 的 控制 方法 之 一 是 基于 神经 网 
络 的 智能 控制 器 ,其 利用 神经 网 络 的 强大 的 通用 近似 能 力 来 补偿 未 知 的 动力 学 。 

应 当 注 意 ,一 般 的 神经 网 络 控 制 方法 仅 确保 闭环 信号 的 最 终 有 界 意义 上 的 稳定 性 ,因为 
神经 网 络 的 近似 仅 保持 在 某 个 紧凑 的 集合 (所 谓 的 神经 网 络 的 近似 域 ) 上 。 因 此 ,必须 事先 
精确 地 识别 状态 变量 的 范围 ,特别 是 对 于 具有 多 输入 和 多 输出 的 高 度 非 线性 复杂 系统 。 因 
此 ,开发 具有 全 局 稳定 性 的 神经 网 络 控制 器 是 非常 重要 的 。 然 而 ,在 大 多 数 现 有 工作 中 仅 考 
虑 单 输入 单 输出 系统 ,并 且 很 少 同时 考虑 瞬 态 性 能 。 

本 节 的 目标 是 通过 利用 障碍 Lyapunov 函数 (BLFs) 实 现 事 先 规定 的 瞬 态 性 能 ,并 且 同 
时 保证 全 局 稳定 性 。 基 于 BLF 的 控制 器 被 开发 用 于 控制 具有 关节 空间 约束 的 机 器 人 机 
械 臂 。 

注意 ,通过 对 状态 或 输出 的 行为 构成 约束 ,可 以 使 用 BLF 的 技术 间接 约束 跟踪 误差 。 
由 此 ,在 本 文中 ,BLF 技术 是 用 来 实现 瞬 态 和 稳 态 的 目标 跟踪 性 能 。 与 稳 态 响应 的 调节 相 
比 , 瞬 态 控 制 的 调节 要 困难 得 多 。 通 过 构造 规定 的 跟踪 性 能 要 求 函数 ,为 双 辟 机 器 人 的 控制 
器 合成 提出 了 适当 的 BLF ,使 得 可 以 确保 瞬 态 和 稳 态 跟踪 性 能 。 同 时 ,在 神经 网 络 控制 器 
设计 中 引入 了 切换 机 制 , 以 确保 全 局 稳定 性 , 见 图 7-28。 与 只 保证 SGUUB 稳定 性 的 传统 神 
经 网 络 控制 器 相 比 ,我 们 提出 的 神经 网 络 控制 器 保证 了 闭环 系统 的 全 局 稳定 性 , 见 图 7-29. 
这 更 有 利于 实际 的 应 用 ,因为 神经 网 络 输入 的 条 件 被 极 大 地 放宽 。 


-神经 网 络 
5s, EMRO 


0 005 0.1 0.15 02 0.25 03 035 7 aa 
图 7-28 跟踪 误差 和 性 能 函数 之 间 的 关系 图 7-29 全 局 跟踪 性 能 


7.3.1 实验 设计 与 实现 
1. Python 代码 
以 下 关节 空间 控制 器 和 PID 控制 器 的 定义 有 所 不 同 , 程 序 代码 如 下 : 


class CJointController: 
def init (self, ArmName): 
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self.name = ArmName 

self.qd = np.mat([[0.0],[0.0],[0.0],[0.0],[0.0],[0.0],[0.0]]) 

self. dotqd = np.mat([[0.0],[0.0],[0.0],[0.0],[0.0],[0.0],[0.0]]) 

# self.qcmd = dict() 

self.limb = baxter interface. Limb(ArmName) 

self.limb.set joint position speed(1.0)self.limb.set joint position speed(0.5) 
self.torController0 = CPIDController(self. name + 'j0')CPIDController() 
self.torControllerl = CPIDController(self. name + 'j1')CPIDController() 
self.torController2 = CPIDController(self. name + 'j2')CPIDController() 
self.torController3 = CPIDController(self. name + 'j3')CPIDController() 
self.torController4 = CPIDController(self. name + 'j4')CPIDController() 
self.torController5 = CPIDController(self. name + 'j5')CPIDController() 
self.torController6 = CPIDController(self. name + 'j6')CPIDController() 
self.torController - [self.torController0, self.torControllerl, 
self.torController2, self.torController3, self.torController4, self.torController5, 
self. torController6] 


self.krate - 1.0 


" 
o 
E 
o 


self. torController[0].kp 
self.torController[0].kd 
# self. torController[0].ki = 0.01 


" 
o 
o 
X 
N 
> 
o 


self.torController[1].kp 60 
self.torController[1].kd 6.02: 24.0 
# self.torController[1].ki = 0.01 


self.torController[2].kp = 50.0 
self.torController[2].kd 5.03:18.0 
# self.torController[2].ki = 0.01 


self.torController[3].kp 30.0 
self.torController[3].kd 3.03: 15.0 
Zself.torController[3].ki = 0.01 


self.torController[4].kp = 11.0 
self.torController[4].kd = 1.1#5.4 
Zself.torController[4].ki = 0.01 


self.torController[5].kp 
self.torController[5].kd 
Zself.torController[5].ki = 0.01 


[] 


self.torController[6].kp 
self.torController[6].kd 
£self.torController[6].ki = 0.002 


LEX LLLLLLLCCCCULC dd 


def _ init__(self, name): 


init 


self.kp - 1.0 
self.ki - 0.0 
self.kd - 0.0 
Self. sum max 


" 
m 
o 
eo 
= 
o 


self. sum min = -— 1000.0 
self.err_old = 0.0 
self.err_sum = 0.0 
self.derr_old = 0.0 
self. sumrate = 1.0 
self.rho = 0.1 
self.name - name 


timestr = time.strftime(" $M&H— %d% m") 
self.file - open(timestr * self.name * ".csv", 


(self): 
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井 井 井 # 井 井 井 井 井 PID Controller 提 提 提 打 打 打 宁 提 提 打 提 打 提 打 提 宁 提 宁 提 打 提 打 打 捍 打 宁 失 
class CPIDController: 


"w) 


tempstr = "time, given, dotgiven, feedback, dotfeedback, err, derr, errtr, outV rV n" 


self.file.write(tempstr) 


def calculate dot(self, given, dotgiven, feedback, dotfeedback): 


err = given - feedback 


derr - dotgiven - dotfeedback 


self.derr old = (self.derr old» 1.0 + derr)/2.0 


self.err old - given 
self.err sum += err 


if self.err sum > self.sum max * self.sumrate: 
self.err sum = self.sum max * self. sumrate 

elif self.err sum < self.sum min * self.sumrate: 
self.err sum = self.sum min* self. sumrate 


#out = self.kp x err + self.kd x derr + self.ki x self.err sum 
out = self.kp * err + self.kd * self.derr old + self.ki * self.err sum 
tenpstr = str(rospy.get time()) + "," + str(given) t "," * str(dotgiven) t "," + 


self.file.write(tempstr) 
return out 


str(feedback) + "," + str(dotfeedback) + "," + str(err) * "," + str(derr) * "," + str(err) +", 
" str(out) * ", XXn" 


def calculate dot tr(self, given, dotgiven, feedback, dotfeedback): 
Ztempstr = "time, given, dotgiven, feedback, dotfeedback, err, derr, errtr, out\r\n" 


err = given — feedback 
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derr = dotgiven — dotfeedback 
self.derr old = (self.derr old * 1.0 + derr)/2.0 
self.err old - given 
self.err sum += err 
errtemp - err 
if errtemp > 0.99999 x self. rho: 
errtemp = 0.99999 x self. rho 
if errtemp < — 0.99999 x self. rho: 
errtemp = -— 0.99999 * self. rho 
errtr = 0.5 * self. rho * np. log( (errtemp/self.rho + 1.0)/(1.0 — errtemp/self. rho)) 
if errtr > 0.2: 
errtr — 0.2 
if errtr « —0.2: 
errtr = -0.2 
if self. err_sum > self. sum_max * self. sumrate: 
self.err sum = self. sum max * self. sumrate 
elif self.err sum < self. sum_min * self. sumrate: 
self.err_sum = self. sum min» self. sumrate 
#out = self.kp * err + self.kd * derr + self.ki * self.err sum 
out = self.kp * errtr + self.kd * self.derr old + self.ki * self.err sum 
tempstr = str(rospy.get time()) + "," * str(given) t "," t str(dotgiven) + "," + 


str(feedback) + "," + str(dotfeedback) * "," + str(err) * "," + str(derr) * "," + str(errtr) 
uu) EE DS 


self.file.write(tempstr) 
return out 


2. MATLAB 代码 

以 下 3 个 m 文 件 是 共同 工作 的 ,它们 的 作用 是 生成 机 器 人 的 轨迹 ,并 转化 为 机 器 人 末 
端 执行 器 的 轨迹 ,其 中 定时 器 起 到 了 控制 时 序 作用 。 

COD 轨迹 设置 相关 代码 如 下 : 


clear all 


close all 


globalt; 
t=0; 


us = udp('192.168.1.100',6101); 
fopen(us); 


ti= timer(); 

set(ti, 'BusyMode', 'drop', 'Period',0.1, 'ExecutionMode', 'fixedRate') 
set(ti, 'TimerFcn', ( (à tineChange, us] ) 

start(ti); 


E 
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(2) 轨迹 空间 转换 代码 如 下 : 


function baxterPos = moveBaxHand twoarms(us, leftpos, rightpos, handName) 

多 移动 baxter 的 手臂 到 坐标 baxxyz。 

* baxxyz 为 在 baxter 坐标 系 中 期 望 位 置 坐标 , handName 为 想 要 移动 的 手臂 名 称 , 分 别 为 left, right 
和 both 

lftrpy- [pi 0 pi]'; 

rgtrpy- [pi 0 pi]'; 

% lftrpy- [-pi/200.1]'; 

* rgtrpy- [ - pi/20 0]'; 

al = size(leftpos); 

a = size(rightpos); 


if al(1) ==1 &al(2) ==3 名 自动 调整 坐标 为 列 向 量 ,因为 下 文中 的 encoder 要 求 为 列 向 量 
leftpos = leftpos'; 
elseif al(1)==3 &al(2)==1 
leftpos; 
else 
display( 'baxxyz error') 
baxterPos=[0 0 0]; 
return; 
end 


if a(1) ==1 & a(2) == S ELI REA c A di, 因为 下 文中 的 encoder 要 求 为 列 向 量 
rightpos = rightpos'; 
elseif a(1) ==3&a(2)== 
rightpos; 
else 
display( 'baxxyz error') 
baxterPos- [0 0 0]; 
return; 
end 


if isequal(handName, right') ”多 移动 左手 或 右手 或 判断 移动 哪 只 手 
[data] = encoder1( [0.5805 0.2625 0.2141] ', rightpos, 1ftrpy, rgtrpy, [255 255]) ; 
fwrite(us, data); 
elseif isequal(handName, 'left') 
[data] = encoderi(leftpos,[0.5735 — 0.224 0.2141]', 1ftrpy, rgtrpy, [255 255]); 
fwrite(us, data); 
elseif isequal(handName, 'both') 
% [y z j] = theloop2(baxxyz) ; 
[data] = encoderl( leftpos, rightpos, lftrpy, rgtrpy, [0 255 ]); 
fwrite(us, data); 
end 
baxterPos- [0 0 0]; 


end 
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(3) 定时 器 相关 代码 如 下 : 


function timeChange(obj, event, us) 
% UNTITLED2 Summary of this function goes here 
% Detailed explanation goes here 
globalt; 
t=t+ obj. Period; 
length= 0.1; 
tstep= 2.5; 
r1=0.12; 
r2=0.1; 


xstartl- 0.65; 
xstartr = 0.65; 
ystartl- 0.2; 

ystartr =- 0.2; 


% * circular trajectroy 

% experiment 1 
leftpos = [rlx sin(t*1 * pi/tstep) + xstartl, r2 * cos(t x 1 x pi/tstep) + ystart1,0.2]; 
rightpos = [r1 » sin(t* 1 * pi/tstep) + xstartr,r2 * cos(t * 1 x pi/tstep) + ystartr,0.2]; 


* 第 第 set point 
moveBaxHand twoarms 3joints(us, leftpos, rightpos, 'both'); 


t 
end 


3. Simulink 模块 

Decoderl 模块 和 Decoder? 模块 的 作用 是 将 UDP ffe sí (19 c4 f (E OS WUE A ZEE AA 
臂 的 关节 角 、 角 速度 . 角 加 速度 .控制 增益 等 ,然后 在 通过 其 他 Simulink 模块 实现 设计 的 控 
制 算法 。 

(1) Decoderl 模块 的 m 代码 如 下 : 


function [left lamda, left qd, left dqd, left q, left dq, right lamda, right qd, right dad, 
right q, right dq, left torque,right torque,left exforce,right exforce]- fcn(data, N) 
persistent Data; 
if isempty(Data) 
Data 7 single(data); 
end 
left lamda = [0000 00 0]'; 
left gd = [000000 0]'; 
left dad = [000000 0]; 
left q = [000000 0]'; 
left dq = [00000 0 0]'; 


第 7 章 ” 机 器 人 操作 系统 的 应 用 | 643 


right lamda = [000000 0]'; 
right qd = [000000 0]'; 
right dqd = [000000 0]'; 
right q = [00000 0 0]'; 
right dq = [000000 0]'; 
left torque- [000000 0]'; 
right torqe- [0 0 0 0 0 0 0]'; 
left_exforce= [0 0 0 0 0 0]'; 
right_exforce= [0 0 0 0 0 0]'; 


ifN—-0 

Data - single(data); 
end 
fori - 0:6 


left lamda(i*1) = (Data(142»* i) +Data(2+ 2 x i) x 256)/1000.0; 

left qd(i+1) = (Data(152»* i) *Data(16 *2* i) «256 — 32768)/10000.0; 
left dqd(i+1) = (Data(29 +2» i) *Data(30 2« i) «256 — 32768)/10000.0; 
left q(i*1) = (Data(43 +2» i) * Data(44 +2» i) «256 — 32768)/10000.0; 
left dq(i*1) = (Data(57 +2» i) + Data(58 +2 i) « 256 — 32768)/10000.0; 


right lamda(i-*1) = (Data(71 +2» i) * Data(72 * 2 x i) » 256)/1000.0; 
right qd(i*1) = (Data(85 +2» i) -*Data(86 +2» i)*256 - 32768)/10000.0; 
right dqd(i+1) = (Data(99 +2» i) Data(100-* 2» i) «256 — 32768)/10000.0; 
right q(i*1) = (Data(113 +2» i) *Data(114 *2* i) «256 - 32768)/10000.0; 
right dq(i+1) = (Data(127 *2* i) + Data(128 + 2 » i) « 256 — 32768)/10000.0; 
end 
for i-0:6 
left torque(i+1) = (Data(141 +2 i) *Data(142 +2 i) * 256 — 32768)/1000.0; 
right torque(i* 1) = (Data(155 + 2» i) +Data(156 + 2» i) *256 — 32768)/1000.0; 
end 
for i-0:5 
left exforce(i-* 1) = (Data(169 + 2» i) +Data(170 +2» i) *256 - 32768)/1000.0; 
right exforce(i* 1) = (Data(181 +2» i) * Data(182* 2» i) x 256 — 32768)/1000.0; 
end 


(2) Decoder2 模块 的 m 代码 如 下 : 


function [leftpos, rightpos, leftrpy, rightrpy, grippers, leftposd, rightposd, leftxyzd, 
rightxyzd]- fcn(data, N) 
persistent Data; 
if isempty(Data) 
Data = single(data); 
end 
leftpos - [00 0]'; 
rightpos = [00 0]'; 
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leftposd = [00 0]'; 
rightposd = [0 0 0]'; 
leftrpy = [00 0]'; 
rightrpy = [00 0]'; 
grippers = [0 0]'; 
leftxyzd= [0 0 0]'; 
rightxyzd= [0 0 0]'; 


Data = single(data); 

for i = 0:2 
leftrpy(i+1) = (Data(7 +2» i) +Data(8 +2» i)%»256 — 32768)/10000. 0; 
leftpos(i+1) = (Data(13+ 2» i) *Data(14 +2» i) x 256 — 32768)/10000.0; 
rightrpy(i-*1) = (Data(25-*2* i) Data(26 +2» i) x 256 — 32768)/10000.0; 
rightpos(i- 1) = (Data(31*2 » i) + Data(32 +2» i) 256 - 32768)/10000.0; 
leftposd(i-1) = (Data(45 *2* i) + Data(46 +2% i) « 256 — 32768)/10000.0; 
rightposd(i* 1) = (Data(51 +2 i) *Data(52*2* i) « 256 — 32768)/10000.0; 
leftxyzd(i + 1) = (Data(57 + 2 * i) + Data(58 +2 » i) « 256 — 32768)/10000.0; 
rightxyzd(i + 1) = (Data(63 + 2 * i) + Data(64 + 2x i) « 256 — 32768)/10000. 0; 


end 


grippers(1) 
grippers(1) 


Data(37); 
Data(38); 


7.3.2 实验 结果 


用 于 实验 操作 对 象 是 Baxter 双 臂 机 器 人 ,如 图 7-30 所 示 。 它 应 用 了 两 个 7 自由 度 机 械 
臂 和 先进 的 传 感 技术 ,包括 位 置 . 力 和 力矩 伟 
感 器 和 控制 ,传感器 安装 在 每 个 关节 处 。 关 
节 传感器 的 分 辩 率 为 14 位 , 共 360 度 ( 每 刻度 
分 辩 率 为 0.022 度 ), 而 可 施加 到 关节 的 最 大 
关节 扭矩 为 5ON 。m( 前 四 个 关节 ) 和 15N + m 
(最 后 三 个 关节 )。 

在 实验 中 ,命令 Baxter 机 器 人 通过 使 用 WESS: 
两 个 机 械 辟 来 抓 取 物 体 , 夹 持 器 安装 在 末端 4 
执行 器 上 的 。 对 于 每 个 机 器 臂 , 我 们 初始 化 
关节 的 位 置 以 使 辟 位 于 如 图 7-30 所 示 的 水 平 图 7-30 实验 装备 的 插图 
面 中 。 在 实验 中 ,为 了 简单 和 不 失 普遍 性 ,使 
用 每 个 辟 上 的 3 个 平行 旋转 接头 (w ,ea ya) 来 规划 运动 。 被 抓 取 的 物体 是 由 塑料 制 成 的 贺 
简 , 重 量 为 0. 1kg ,长度 为 0. 1m, 直 径 为 0. 06m。 内 力 可 以 通过 使 用 安装 在 每 个 关节 的 扭矩 
传感器 以 及 重力 补偿 模型 和 公式 来 计算 。 

为 了 很 好 地 近似 机 器 人 动力 学 和 考虑 到 精度 和 计算 效率 ,将 RBFNN 的 输入 分 为 2 组 ， 
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一 组 包含 [qi ,ez € R^ 和 另 一 组 为 [qi di a). ER ,并 且 对 于 神经 网 络 的 每 个 输入 维度 
使 用 三 个 中 心 ,最 终 每 个 神经 网 络 总 共 具 有 5 — 20412 个 神经 网 络 节点 。 神 经 网 络 节点 的 
中 心 被 均匀 地 间隔 开 , 根 据 每 个 关节 的 运动 范围 和 速度 限制 的 上 限 和 下 限 ,如 [一 1.7,1. 7] X 
[—1.05,2. 61] X [—1.57,2.09]X[—1.5,1. 5] X [71.5,1.5]X[—1. 5.1. 5]UL— 1. 7, 
1.7]X[—1.05,2. 61] X [—1. 57,2. 09] X [—1.5,1.5]X 1. 5,1.5]XE—1.5,1.5]X 


[一 1.5,1.5]X[ 一 1.5,1.5]X[ 一 1.5,1.5]。 并 且 神经 网 络 权重 矩阵 被 初始 化 为 砂 ;(0) = 


0€ E^ 8W.()—9€ 2*9, £s RA EL IRE GJ 2E YEFEOU O: = diag {2}, 8 = 
diag{2}。 控 制 器 的 设计 参数 K 和 Kz 规定 为 Ki 二 diag{10,9,9)} K2 — Ko; — diag(9.4. 5, 
1.2}。 控 制 器 中 的 参数 @@ 被 选择 为 6 二 0. 1。 

在 实验 中 , 夹 持 的 物体 需要 在 笛 卡 儿 空间 中 跟踪 以 下 轨迹 
0. 65 +0. 1sin(2x/50) 


d 
y|- 0. 12cos(2x/50) (7.5) 
0 0 


对 象 的 初始 坐标 为 (0. 55,0. 2,0. 2) ,初始 速度 设置 为 (0) 二 0,3(0) —0.0 (0 —0, Br 
需 的 内 力 选 择 为 fa 二 [0,3,0], fa 二 [0, 一 3,0]。 设 计 性 能 函数 的 参数 为 : p =p =0. 2, 
pos =0. 4 spot pe —0. 012, p-s =0. 025 242. 5 fu — Ba =1,k=1,2,3. 

实验 结果 见 图 7-31 一 图 7-34。 被 操纵 对 象 在 任务 空间 中 跟踪 性 能 如 图 7-31 所 示 。 其 
中 观察 到 当 遵 循 圆 形 轨 迹 时 所 提出 的 控制 器 具有 良好 的 性 能 。z、y 和 0 的 轨迹 见 图 7-28(a)、 


一 x 实际 一 J 实际 

08 Xa 0.15 一 yiii 
0.1 
207 ~ 00 
G g 05 
* og LO. 
-0.05 
0.5 n 

QU & GU Js x9 x38 "915p 0 i$ 20 25 
time(s) 


(a) x 轨迹 


0 puce m 
08 
-01 Tes 07 075 


(0 5. 10 i15 20 25 "e "gs ss 06 
time(s) ü 
(c) 6 轨迹 (d) 任务 空间 轨迹 


图 7-31 被 操纵 对 象 的 全 局 稳定 跟踪 性 能 
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(b) (c) 中 给 出 。 被 操纵 物体 的 跟踪 误差 示 于 图 7-30(a)、(b)、(c)。 如 这 些 图 所 示 , 所 抓 取 
的 对 象 非常 好 地 遵循 参考 轨迹 ,跟踪 误差 收敛 到 零 附近 ,而 不 违反 预先 规定 的 瞬 态 边界 ( 即 
红色 虚线 ”-")。 控 制 输入 ,内 力 误差 ,关节 位 置 和 神经 网 络 权 重 范 数 的 轨迹 如 图 7-33 和 


图 7-34 所 示 。 从 图 中 可 以 看 出 ,闭环 信号 是 有 界 的 ,并 且 内 力 误差 收敛 到 零 附近 。 


一 在 提出 的 控制 器 下 


el(m) 


02 


一 在 提出 的 控制 器 下 
-一 规定 的 界限 
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time(s) time(s) 
(a) (b) 
oA 一 在 提出 的 控制 器 下 0.2 一 在 控制 器 w(D) 下 
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ar EY: 
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-0.1F | 
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图 7-32 对象 操作 的 跟踪 误差 
注 : 图 7-32(a) 、(b) 、(e) 为 设计 控制 器 的 跟踪 误差 ; 图 7-32(d) Ce), (f) 为 修正 后 的 控制 器 u COR uz (t) 的 跟踪 误差 。 
此 外 ,两 个 改进 的 控制 器 的 实验 结果 在 图 7-30(d)、(f) 中 进行 了 比较 (没有 神经 网 络 适 
应 的 us (4) 控制 器 以 及 没有 了 瞬 态 和 神经 网 络 控 制 的 wz(2) 控 制 器 ) 。 
如 这 些 图 所 示 ,在 不 使 用 神经 网 络 控制 和 瞬 态 控制 的 情况 下 ,跟踪 误差 超越 了 规定 的 瞬 态 
边界 ,而 在 不 使 用 神经 网 络 控制 的 情况 下 观察 到 相对 较 大 的 稳 态 误差 。 实 验 结果 表明 ,我 们 提 
出 的 控制 器 可 以 成 功 地 保证 跟踪 误差 保留 在 预定 义 区 域 ,并 确保 规定 的 瞬 态 边界 从 不 超越 。 
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图 7-33 Baxter 机 器 人 的 控制 输入 和 内 力 误差 


Wille: 一 Ia 
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time(s) 
(a) 关节 角度 9y (b) 神经 网 络 权 值 范 数 8 天 12, 产 123) 


图 7-34. 九 关节 位 置 的 轨迹 和 神经 网 络 权 值 的 收敛 


25 


7.4 基于 人 体 运 动 捕获 对 Baxter 机 器 人 的 远程 操作 控制 


本 节 介绍 基于 人 体 运动 捕获 对 Baxter 机 器 人 的 远程 操作 控制 的 系统 架构 、 编 程 与 实现 。 

在 过 去 几 十 年 ,机 器 人 在 制造 业 中 的 应 用 越 来 越 广 泛 。 机 器 人 可 以 在 特定 环境 中 代替 
人 类 完成 重复 和 危险 的 工作 。 但 是 用 户 需要 通过 外 部 设备 才能 对 机 器 人 进行 操作 ,因此 会 
增加 操作 的 复杂 度 和 给 用 户 带 来 不 便 。 如 果 机 器 人 能 够 学 会 人 类 的 技能 和 适应 环境 的 能 
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力 ,那么 控制 效率 会 大 大 提高 。 最 直接 的 方法 是 让 机 器 人 实时 跟踪 人 体 的 运动 。 

为 了 实现 这 一 目标 ,首先 需要 捕获 人 体 的 运动 。 现 阶段 ,最 常用 的 方法 是 在 人 体 上 安装 
跟踪 传感器 。 例 如 ,传感器 安装 在 人 体 的 皮肤 上 ,从 而 检测 骨骼 的 运动 和 两 相 邻 部 分 的 相对 
运动 。 然 而 ,这 些 设备 不 仅 价格 高 晶 ,而 且 不 便利 的 操作 还 会 限制 实际 的 使 用 效果 。 

如 今 ,基于 视觉 的 运动 捕获 系统 提供 了 一 类 新 的 人 体 运动 捕获 方法 。 例 如 ,摄像 头 可 以 
代替 可 穿戴 设备 来 对 人 体 运 动 进行 捕获 ,因而 用 户 可 以 摆脱 那些 累 歼 的 穿戴 设备 。 基 于 视 
觉 的 运动 捕获 系统 不 仅 功能 强大 ,而 且 十 分 便利 ,已 广泛 应 用 于 机 器 人 之 中 。 

在 直角 坐标 系 中 ,Kinect 传感器 能 够 检测 25 个 人 体 关节 点 。 在 直角 坐标 系 中 检测 到 人 
体 关节 数据 之 后 ,需要 将 其 转换 为 关节 坐标 系 中 的 数据 。 

在 实际 操作 中 ,系统 会 存在 几 个 不 确定 的 参数 。 系 统 主要 的 不 确定 性 在 于 未 建 模 部 分 
的 动态 性 能 ,这 会 对 控制 效果 造成 影响 。 但 是 ,基于 准确 模型 的 传统 机 器 人 控制 方法 并 不 能 
达到 我 们 的 控制 要 求 。 为 了 解决 这 个 问题 ,我 们 提出 一 种 基于 函数 逼近 法 (FAT) 的 近似 控 
制 方法 。 这 种 近似 控制 的 方法 利用 FAT 可 以 不 断 逼 近 系统 动态 ,因而 广泛 应 用 在 控制 系 
统 中 。 系 统 参数 可 以 通过 在 线 学 习 算法 来 估计 ,以 保证 期 望 的 控制 效果 。 


7.4.1 远程 操作 控制 系统 


1. 系统 组 成 

本 控制 系统 主要 由 关节 角度 位 置 的 检测 部 分 、 数 据 传输 通道 和 基于 FAT 的 控制 器 组 
JR. Fd 7-35 是 本 远程 操作 系统 的 流程 图 。 由 Kinect V2 检测 到 的 人 体 手臂 关节 状态 作为 
参考 输入 ; 机 器 人 为 被 控 对 象 ; 其 输出 为 反馈 信号 。 可 以 通过 控制 器 不 断 修正 偏差 。 
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图 7-35 远程 操作 系统 流程 图 


2. Kinect 
微软 公司 第 二 代用 于 Windows 系统 的 产品 Kinect V2 功能 强大 而 且 价 格 低廉 ,已 广泛 
应 用 于 控制 系统 中 ,用 以 捕获 三 维 运动 数据 。 第 二 代 Kinect 传感器 包含 一 个 麦克 风 阵 列 、 
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一 个 RGB 摄像 头 以 及 一 个 深度 传感器 ,如 图 7-36 所 示 。RGB 摄像 头 可 以 收集 物体 的 光线 
并 将 其 转换 为 数字 信号 ; 深度 传感器 可 以 获取 物体 的 深度 图 像 。 在 遥 操 作 控制 系统 中 ,人 
体 可 以 看 作 由 25 个 关节 点 构成 的 骨架 ,包括 蔷 部 \. 肩 部 以 及 肘 部 等 。 图 7-37 为 人 体 关 节 的 
三 维 空间 图 。 区 别 于 传统 的 基于 RGB 数据 的 运动 捕获 系统 ,该 系统 无 须 依靠 复杂 的 编程 技 
术 。 利 用 开源 的 软件 开发 包 ,设备 可 以 直接 应 用 于 迁 操 作 控制 系统 当中 。 


彩色 相机 深度 传感器 


A / 
麦克 风 阵列 


图 7-36 Kinect V2 传感器 外 观 图 图 7-37 人 体 关 节 的 三 维 空间 图 


3. Kinect DevelopSoftware 

许多 软件 都 为 Kinect 传感器 开发 者 提供 开发 平台 。Kinect SDK 2. 0 for Windows 就 
是 一 种 由 微软 公司 提供 的 开发 工具 。 开 发 者 可 以 使 用 Kinect 软件 开发 包 扩 展 Kinect 传 感 
器 设备 的 应 用 。 开 发 平台 支持 Windows 8 以 上 的 系统 。 

开发 软件 需要 在 Windows 环境 下 工作 ,并 且 能 与 Kinect 进行 信息 交互 。 而 且 要 求 能 
够 计算 从 Kinect 提取 出 来 的 骨 铝 运动 数据 ,并 将 其 发 送 到 Linux 平台 的 开发 工作 区 间 ,来 
对 Baxter 机 器 人 进行 控制 。 因 此 ,我 们 选择 能 满足 以 上 开发 要 求 的 Microsoft Visual C++, 
这 款 软件 能 够 与 Kinect 进行 交互 ,并 检测 人 体 骨 骼 信和 号。 

4. 机 器 人 操作 系统 ROS 和 Rospy 

ROS (机 器 人 操作 系统 ) 是 一 款 机 器 人 软件 平台 ,为 用 户 提供 一 些 标准 的 操作 系统 服 
务 。ROS 可 以 分 为 两 层 , 下 层 为 操作 系统 层 , 而 上 层 则 是 为 用 户 实现 不 同 功能 而 配备 的 软 
件 包 ,例如 映射 .动作 规划 、 视 觉 和 仿真 等 。Rospy 是 稳 态 ROS 核心 发 行 版 的 一 部 分 ,为 程 
序 员 提 供 接口 。 

5. Baxter 机 器 人 

Baster 机 器 人 是 Robotics Rethink 公司 生产 的 一 
款 具 有 学 习 能 力 的 工业 机 器 人 。 它 不 需要 专门 的 机 器 
人 专家 或 者 复杂 的 用 户 程 序 来 完成 生产 操作 任务 。 这 
款 机 器 人 是 针对 一 系列 不 同 的 简单 工业 任务 而 设计 出 
来 的 。 如 图 7-38 所 示 ,Baster 机 器 人 高 约 3 英尺 .拥有 - 
双 臂 和 卡通 人 物 脸 。 机 器 人 手臂 拥有 7 个 自由 度 ( 肩 关 图 7-38 ”Baxter 机 器 人 
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节 : sj0,sjl,sj2; 肘 部 关节 : ej0, ejl; 手腕 关节 : wj0,wjl,wj2)。 在 扭矩 -转角 模式 下 ,每 个 
关节 都 能 独立 驱动 和 控制 。 
D-H 人 体 手臂 模型 见 图 7-39. 


图 7-39 D-H 人 体 手 臂 模 型 


人 体 手 臂 模型 参数 列表 见 表 7-1 。 
表 7-1 人 体 手 臂 模型 参数 列表 


i 4 d; a; a; 
0 一 /2 0 0 —n/2 
1 $i (2/2) 0 0 —n/2 
2 gi C) 0 0 一 Kx/2 
3 ar/2) d; 0 x/2 
4 g (0) 0 0 —n/2 
5 qs C7 2/2) d; 0 —n/2 
6 gs C x/2) 0 0 —n/2 
1 g0) 0 d7 x 
基于 向 量 法 的 角度 计算 见 图 7-40, 

ye 

1 

aM - N 左 拇指 


图 7-40 基于 向 量 法 的 角度 计算 
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7.4.2 实验 的 设计 与 实现 

本 节 内 容 所 设计 的 框架 如 图 7-41 所 示 , 其 中 C++ 程序 用 于 接收 Kinect 传感器 传输 的 数 
据 , 并 进行 处 理 ,生成 关节 角度 qao Python 程序 主要 是 围绕 主 控制 器 而 设计 的 ,对 关节 角度 
进行 处 理 后 输出 力矩 r, 而 MATLAB/Simulink 运用 神经 网 络 控制 算法 ,系统 的 未 知 项 进行 
估计 ,相当 于 前 馈 控 制 ,并 生成 了 补偿 力矩 re。 


| Kinect =| CH 程序 o  Pyinonter 


图 7-41 实验 程序 框架 


1. Python 代码 

该 Python 程序 主要 实现 的 功能 是 接收 并 处 理 Kinect 发 送 的 相关 数据 。 它 将 接收 到 的 
字符 串 型 数据 改 为 浮 点 型 数据 ,并 将 人 体 的 关节 空间 映射 到 机 器 人 运动 的 关节 空间 ,已 达到 
使 用 Kinect 播 操作 机 器 人 的 目的 ,程序 代码 如 下 : 


class CudptoFFAT: 

def _ init__(self, leftarm, rightarm): 
self.HOST = '192.168.1.100' 
self.PORT - 6103 
self.BUFSIZE - 1024 
self.ADDR = (self. HOST, self. PORT) 
self. RemotePORT = 6103 
self.udpSock = socket. socket(socket.AF INET,socket.SOCK DGRAM) 
self.udpSock. settimeout(1) 
self. udpSock. bind( self. ADDR) 
self. connected = False 
self.t = threading. Thread(target = self.listenWorker) 
self.thread stop = False 
self.leftarm - leftarm 
self.rightarm - rightarm 

self.PI= 3. 14159267 
self.qd = np.mat([[0.0], [0.0], [0.0], [0.0], [0.0], [0.0], [0. 0]]) 
self.errcounter = 0 
self. t. start() 


def Decode( self, data) : 
self.FATResultLeft[0, 0] 
self.FATResultLeft[1,0] 


(ord(data[0]) * 256 * ord(data[1])) - 32768.0)/1000.0 


E 
= ((ord(data[2]) * 256 + ord(data[3])) - 32768. 0)/1000.0 
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1000.0 


1000.0 


1000.0 


1000.0 


1000.0 


1000.0 


1000.0 


self.FATResultLeft[2, 0] 
self.FATResultLeft[3,0] 
self.FATResultLeft[4,0] 
self.FATResultLeft[5, 0] 
self.FATResultLeft[6, 0] 


self. FATResultRight[0,0] - 


self. FATResultRight[1,0] - 


self. FATResultRight[2,0] - 


self. FATResultRight[3,0] - 


self. FATResultRight[4,0] = 


self. FATResultRight[5,0] - 


self. FATResultRight[6,0] = 


def listenWorker(self): 


udp rate = rospy.Rate(1000.0) 
while not self.thread stop: 


try: 


((ord(data[14]) * 256 + ord(data[15])) - 32768. 


((ord(data[16]) * 256 * ord(data[17])) - 32768 


((ord(data[18]) * 256 + ord(data[19])) - 32768. 
((ord(data[20]) * 256 + ord(data[21])) - 32768. 
((ord(data[22]) * 256 + ord(data[23])) - 32768. 
((ord(data[24]) * 256 + ord(data[25])) - 32768. 


((ord(data[26]) * 256 * ord(data[27])) - 32768. 


data, addr = self. udpSock. recvfron(self.BUFSIZE) 
self.connected - True 


# print data 
# self. Decode(data) 


qd = np.mat([[0.0], [0.0], [0.0], [0.0], [0.0], [0.0], [0.0]]) 


qd[0,0] = float(data[0:8]) 
qd[1,0] = f1oat(data[9:17] 


) 


qd[ 2,0] = float(data[18:26]) 
qd[ 3,0] = f1oat(data[27:35]) 


qd[4,0] = f1oat(data[36:44 


]) 


qd[ 5,0] = float(data[45:53]) 
qd[ 6,0] = float(data[54:62]) 


qd[0,0] =- 1.7 + ad[ 0,0]/self.PI*1.7 


qd[1,0] = 


05 - qd[1,0]/self.PI* 3.19 + 0.5 


qd[2,0] = -3.05 + ad[2, 0] /self.PI « 3.05 
qd[ 3,0] = 2.61 - qd[3, 0] /self.PI x 2. 66 


qd[ 4,0] = ad[4,0] 
qd[5,0] = ad[5,0] 
qd[6,0] - ad[6,0] 


((ord(data[4]) * 256 + ord(data[5])) ~ 32768. 0)/1000.0 
((ord(data[6]) * 256 + ord(data[7])) - 32768. 0)/1000.0 
((ord(data[8]) * 256 + ord(data[9])) - 32768. 0)/1000.0 
((ord(data[10]) * 256 + ord(data[ 11])) - 32768. 0)/1000. 0 
((ord(data[12]) * 256 + ord(data[ 13])) - 32768. 0)/1000. 0 


0)/ 


.0)/ 


0)/ 
0)/ 
0)/ 
0)/ 


0)/ 


self.leftarm.qkinect = qd 
except: 
self.errcounter += 1 
self.connected - False 


print "FFAT UDP time out. Counter =" 


udp rate. sleep() 


def stop(self): 
self.thread stop - True 
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, Self.errcounter 


LEIELLILLILZZIXILILILILLIZZCZILICICLLLLLCLICLCLCICCLCISE 


LLXIXLILLCILLII 
qd = self. qkinect 
errqk = self. qkinect - qnow 
print 'self.qkinect', self.gkinect, 'errqk = ', errgk 
dotqd = self.poscontroller.calculate mul err(errqgk) 
for i in range(0, 7): 

if dotqd[i] > 0.2: 


dotqd[i] = 0.2 
if qnow[i] < 一 0.2: 
qnow[i] = 0.2 


for i in range(0, 7): 
if qnow[i] > self. qmax[i]: 
qnow[i] = self.qmax[i] 
dotqd[i,0] = 0 
if qnow[i] < self. qmin[ i]: 
qnow[i] = self.qmin[i] 
dotad[i,0] = 0 
LEXLLLLLLCII 2 22 


2. Kinect 捕捉 人 体 骨 骼 
CD 坐标 点 计算 的 代码 如 下 : 


HipRight.x += joint[JointType HipRight].Position.X; 
HipRight.y += joint[JointType_HipRight]. Position. Y; 
HipRight.z += joint[JointType_HipRight]. Position. Z; 


HipLeft.x += joint[JointType HipLeft].Position.X; 
HipLeft.y *- joint[JointType HipLeft].Position.Y; 
HipLeft.z += joint[JointType HipLeft].Position.Z; 


ShoulderRight.x += joint[JointType ShoulderRight].Position.X; 
ShoulderRight.y += joint[JointType ShoulderRight].Position.Y; 
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ShoulderRight.z += joint[JointType ShoulderRight].Position.Z; 


ShoulderLeft.x += joint[JointType ShoulderLeft]. Position. X; 
ShoulderLeft.y += joint[JointType ShoulderLeft].Position. Y; 
ShoulderLeft.z += joint[JointType ShoulderLeft].Position.Z; 


ElbowLeft.x *- joint[JointType ElbowLeft].Position.X; 
ElbowLeft.y += joint[JointType ElbowLeft].Position.Y; 
ElbowLeft.z += joint[JointType _ElbowLeft]. Position. Z; 


WristLeft. joint[JointType WristLeft].Position.X; 
WristLeft.y *- joint[JointType WristLeft].Position.Y; 
WristLeft.z += joint[JointType WristLeft].Position.Z; 


x 
d 
" 


HandLeft.x += joint[JointType HandLeft].Position.X; 
HandLeft.y += joint[JointType HandLeft].Position.Y; 
HandLeft.z += joint[JointType HandLeft]. Position. Z; 


ThumbLeft.x += joint[JointType ThumbLeft].Position.X; 
ThumbLeft.y += joint[JointType ThumbLeft].Position.Y; 
ThumbLeft.z += joint[JointType ThumbLeft].Position.2; 


HandTipLeft.x += joint[JointType HandTipLeft].Position.X; 
HandTipLeft.y += joint[JointType HandTipLeft]. Position. Y; 
HandTipLeft.z += joint[JointType HandTipLeft]. Position. Z; 


ElbowRight.x += joint[JointType ElbowRight].Position.X; 
ElbowRight. = joint[JointType ElbowRight].Position.Y; 
ElbowRight. joint[JointType ElbowRight].Position.Z; 


WristRight.x *- joint[JointType WristRight].Position.X; 
WristRight.y *- joint[JointType WristRight].Position.Y; 
WristRight.z += joint[JointType WristRight].Position.Z; 


HandRight.x += joint[JointType HandRight].Position.X; 
HandRight.y += joint[JointType HandRight].Position.Y; 
HandRight.z += joint[JointType HandRight].Position.2; 


ThumbRight.x += joint[JointType ThumbRight].Position.X; 
ThumbRight.y += joint[JointType ThumbRight].Position.Y; 
ThumbRight.z += joint[JointType ThumbRight].Position.2; 


HandTipRight.x += joint[JointType HandTipRight].Position.X; 
HandTipRight.y += joint[JointType HandTipRight].Position.Y; 
HandTipRight.z += joint[JointType HandTipRight].Position.2; 
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(2) 向 量 和 角度 计算 的 代码 如 下 : 


// 计 算 向 量 ; 
CvPoint3D32f CO, CD, CB, CE, CH, DC, DD1, DE, EC, ED, EH, EF, EG, L X5, L X7; 
//Lefthrn 
GetVector(ShoulderLeft, HipLeft); 
GetVector(ShoulderLeft, ElbowLeft); 
GetVector(ShoulderLeft, ShoulderRight); 
7 GetVector(ShoulderLeft, WristLeft); 
7 GetVector(ShoulderLeft, ThumbLeft); 
7 GetVector(ElbowLeft, ShoulderLeft); 
DD1.x = 0; DDl.y --1; DDl.z = 0; 
7 GetVector(ElbowLeft, WristLeft); 
= GetVector(WristLeft, ShoulderLeft); 
7 GetVector(WristLeft, ElbowLeft); 
7 GetVector(WristLeft, ThumbLeft); 
= GetVector(WristLeft, HandLeft); 
7 GetVector(HandLeft, HandTipLeft); 
Ok =- (EF.x# DE.x + EF. y» DE.y + EF.z* DE.z) / (EH.x * DE.x + EH. y« DE.y + EH.z* DE. z); 
L X5 - VectorPlus(VectorMultiply(Left k, EH), EF); 
L X7 = CrossProduct(EF, CrossProduct(EF, EH)); 
//RightArm 
CvPoint3D32f BA, BK, BC, BL, KB, KL, LK, LP, LN, LM, R X5, R X7; 
BA = GetVector(ShoulderRight, HipRight); 
7 GetVector(ShoulderRight, ElbowRight); 
7 GetVector(ShoulderRight, ShoulderLeft); 
7 GetVector(ShoulderRight, WristRight); 
7 GetVector(ElbowRight, ShoulderRight); 
GetVector(ElbowRight, WristRight); 
7 GetVector(WristRight, ElbowRight); 
7 GetVector(WristRight, ThumbRight); 
7 GetVector(WristRight, HandTipRight); 
7 GetVector(WristRight, HandRight); 
ight k =- (IM.x x K.x + IM. y* K.y + IM.z*K.z) / (LP.x*xKL.x + IP.yx KL. y + LP.z* KL.z); 
= VectorPlus(VectorMultiply(Right k, LP), IM); 
R X7 = CrossProduct(IM, CrossProduct(LM, IP)); 
// 计 算 角度 ; 
/LeftArm 
Leftanglel PI - AngerOf(CrossProduct(CO, CD), CrossProduct(CB, CO)); 
Leftangle2 = AngerOf(CO, CD); 
Leftangle3 = AngerOf(CrossProduct(CO, CD), CrossProduct(CD, CE)); 
Leftangle4 = AngerOf(DC, DE); 
Leftangle5 - AngerOf(CrossProduct(DE, DD1), CrossProduct(ED, EH)); 
Leftangle6 = PI/2 + AngerOf(CrossProduct(EH, EF), ED); 


BHRHHBHHEHRRSGS 
"ow ow" 


5 


EEZGSRBBGEHSHBE 


tel ed 
üt 


[] 
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Leftangle7 = AngerOf(L X5, L X7); 


//RightArm 

Rightanglel = PI - AngerOf(CrossProduct(BA, BK), CrossProduct(BC, BA)); 
Rightangle2 = AngerOf(BK, BÀ); 

Rightangle3 - AngerOf(CrossProduct(BA, BK), CrossProduct(BK, BL)); 
Rightangle4 - AngerOf(KB, KL); 

Rightangle5 = AngerOf(CrossProduct(KL, DD1), CrossProduct(LK, LP)); 
Rightangle6 = PI/2 + AngerOf(CrossProduct(LP, LM), LK); 

Rightangle7 = AngerOf(R X5, R X7); 


(3) 画 出 人 体 骨 骼 的 代码 如 下 : 


void DrawBone(Mat& SkeletonImage, CvPoint pointSet[], const Joint x pJoints, int whichone, 
JointType joint0, JointType jointl) 
{ 

TrackingState joint0State = pJoints[joint0].TrackingState; 

TrackingState jointlState pJoints[ jointl]. TrackingState; 


if ((jointOState == TrackingState NotTracked) || (jointlState == TrackingState 
NotTracked)) 
( 


return; 


if ((jointOState = = TrackingState _ Inferred) && (jointlState == TrackingState _ 
Inferred)) 
t 


return; 


CvScalar color; 
Switch (whichone) // 跟 踪 不 同 的 人 显示 不 同 的 颜色 
{ 
case 0: 
color = cvScalar(255); 
break; 
case 1: 
color = cvScalar(0, 255); 
break; 
case 2: 
color = cvScalar(0, 0, 255); 
break; 
case 3: 
color = cvScalar(255, 255, 0); 
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color = cvScalar(255, 0, 255); 


color = cvScalar(0, 255, 255); 


if ((joint0State == TrackingState Tracked) && (jointlState == TrackingState Tracked)) 
{ 

line(SkeletonImage, pointSet[joint0], pointSet[joint1], color, 2); 
) 


else 


t 
line(SkeletonImage, pointSet[joint0], pointSet[jointl], color, 2); 


void drawSkeleton (Mat& SkeletonlImage, CvPoint pointSet [ ], const Joint * pJoints, int 
whichone) 


t 

DrawBone(SkeletonImage, pointSet, pJoints, whichone, JointType Head, JointType Neck); 

DrawBone (Skeletonlmage, pointSet, pJoints, whichone, JointType Neck, JointType - 

SpineShoulder); 

DrawBone(Skeletonlmage, pointSet, pJoints, whichone, JointType SpineShoulder, JointType 
SpineMid); 

DrawBone(Skeletonlmage, pointSet, pJoints, whichone, JointType SpineMid, JointType _ 
SpineBase); 

DrawBone(Skeletonlmage, pointSet, pJoints, whichone, JointType SpineShoulder, JointType 
ShoulderRight); 

DrawBone(Skeletonlmage, pointSet, pJoints, whichone, JointType SpineShoulder, JointType 
ShoulderLeft); 

DrawBone(SkeletonlImage, pointSet, pJoints, whichone, JointType SpineBase, JointType - 
HipRight); 


DrawBone(Skeletonlmage, pointSet, pJoints, whichone, JointType SpineBase, JointType _ 
HipLeft); 


//Right Arm 

DrawBone(SkeletonImage, pointSet, pJoints, whichone, JointType ShoulderRight, JointType | 
ElbowRight); 

DrawBone(SkeletonlImage, pointSet, pJoints, whichone, JointType ElbowRight, JointType | 
WristRight); 

DrawBone(SkeletonlImage, pointSet, pJoints, whichone, JointType WristRight, JointType . 
HandRight); 
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DrawBone ( SkeletonImage, pointSet, pJoints, whichone, JointType HandRight, JointType_ 
HandTipRight); 

DrawBone(SkeletonlImage, pointSet, pJoints, whichone, JointType WristRight, JointType | 
ThunbRight); 


//Left Arm 

DrawBone(SkeletonImage, pointSet, pJoints, whichone, JointType ShoulderLeft, JointType 
ElbowLeft); 

DrawBone(SkeletonImage, pointSet, pJoints, whichone, JointType ElbowLeft, JointType _ 
WristLeft); 

DrawBone(SkeletonImage, pointSet, pJoints, whichone, JointType WristLeft, JointType | 
HandLeft) ; 

DrawBone(SkeletonImage, pointSet, pJoints, whichone, JointType HandLeft, JointType _ 
HandTipLeft); 

DrawBone(SkeletonImage, pointSet, pJoints, whichone, JointType WristLeft, JointType | 
ThunmbLeft ) ; 


//Right Leg 

DrawBone(SkeletonImage, pointSet, pJoints, whichone, JointType HipRight, JointType _ 
KneeRight) ; 

DrawBone(SkeletonImage, pointSet, pJoints, whichone, JointType KneeRight, JointType __ 
AnkleRight); 

DrawBone(Skeletonlmage, pointSet, pJoints, whichone, JointType AnkleRight, JointType | 
FootRight); 


//Left Leg 

DrawBone (SkeletonlImage, pointSet, pJoints, whichone, JointType HipLeft, JointType _ 
KneeLeft) ; 

DrawBone(Skeletonlmage, pointSet, pJoints, whichone, JointType KneeLeft, JointType _ 
AnkleLeft); 

DrawBone(Skeletonlmage, pointSet, pJoints, whichone, JointType AnkleLeft, JointType | 
FootLeft); 
) 


(4) 相关 的 计算 如 下 : 


// 计 算 向 量 
CvPoint3D32f GetVector(CvPoint3D32f point1，CvPoint3D32f point2) 
{ 

CvPoint3D32f V; 

V.x 7 point2.x = pointl.x; 

V.y = point2.y - pointl.y; 

V.z = point2.z - pointl.z; 

return V; 
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// 计 算 叉 积 
CvPoint3D32f CrossProduct(CvPoint3D32f V1, CvPoint3D32f V2) 
t 

CvPoint3D32f V; 

V.x = VI.y*V2.z — Vl.z V2. y; 

V.y = Vi.z*V2.x - Vl.x* V2.z; 

V.z = VI.x* V2.y - Vl.y* V2.x; 

return V; 


H 
// 计 算 向 量 间 的 夹 角 
float AngerOf(CvPoint3D32f V1, CvPoint3D32f V2) 
t 
float anger; 
anger = acos((Vl.x* V2.x + Vl.y* V2.y + Vl.z* V2.z) / (sart(Vl.x* Vl.x + Vl.yxVl.y + 
Vl.z* Vl.z) «sqrt(V2.x * V2.x + V2.y* V2.y + V2.z*V2.z))); 
return anger; 
} 
// 函 数 ,计算 向 量 加 法 
CvPoint3D32f VectorPlus(CvPoint3D32f pointl, CvPoint3D32f point2) 
{ 
CvPoint3D32f point; 
point.x - point2.x * pointl.x; 
point.y = point2.y + pointl.y; 
point.z - point2.z * pointl.z; 
return point; 
) 
// 函 数 , 计算 向 量 与 数 相 乘 
CvPoint3D32f VectorMultiply(float k, CvPoint3D32f pointl) 
{ 
CvPoint3D32f point; 
point.x - k* pointl.x; 
point.y = kx pointl.y; 
point.z = k* pointl.z; 
return point; 


7.4.3 实验 及 结果 


接 下 来 设计 实验 去 验证 算法 的 效果 。 在 第 一 个 实验 中 ,操作 者 保持 一 个 静态 的 姿势 。 
实验 场景 如 图 7-42 和 图 7-43 所 示 .得 到 人 体 关节 角度 值 并 送 至 MATLAB 软件 进行 处 理 。 
实验 表明 , 当 操 作者 保持 静态 姿势 ,7 个 关节 角 的 检测 结果 稳定 地 处 于 正确 变化 范围 。 其 中 
存在 的 波动 是 由 于 Kinect 检测 的 关节 平面 并 非 绝 对 平稳 以 及 操作 者 自身 无 法 保持 绝对 的 
静止 而 造成 的 少量 偏差 。 虽然 结 果 存 在 波动 .但 结果 仍 处 于 可 接受 范围 。 这 表明 7 个 关节 
角度 计算 方法 可 在 静态 动作 中 效果 良好 。 
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图 7-42 操作 者 静 立 于 Kinect 前 面 , 整 只 手臂 水 平 放 置 
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第 二 个 实验 是 当 操作 者 处 于 动态 时 检测 关节 角度 趋势 。 操 作者 保持 身体 静止 , 慢 速 地 
上 下 运动 肘 部 ,使 其 由 指定 的 起 点 运动 到 指定 的 终点 。 如 第 一 个 实验 的 操作 方法 ,将 数据 送 
至 MATLAB 处 理 肘 部 角度 随 着 人 体 肘 部 的 运动 呈 规 律 性 变化 。 波 形 存在 微小 抖动 的 原因 
是 由 于 操作 者 自身 的 微小 抖动 和 传感器 检测 点 的 不 确定 性 。 总 体 来 说 ,实验 的 结果 与 期 户 
一 致 ,可 以 证 明 该 运动 捕 提 方法 的 正确 性 得 到 人 体 关节 的 角度 数据 后 ,下 面 的 任务 就 是 将 数 
据 进行 变换 再 传送 至 机 器 人 。 

最 后 ,由 于 Baxter 机 器 人 的 坐标 系 和 Kinect 传感器 的 坐标 系 不 同 ,因此 需要 用 转换 和 矩 
Ve T RU EET PE B 将 人 体 关节 角度 转换 为 Baxter 机 器 人 的 关节 角度 : 


Bus = TOrkiee + B (7.6) 
其 中 
" " 
T diag( 1.7 —3.9 —3.05 —2.66 —3.05 —2.09 3.05) an 
* T * x * T 
B= [1.7,1, 55,3,05,2,61,0,01, — 1, 7, — 0, 02]* C.8) 


由 于 人 体 的 7 个 角度 随 操作 者 的 动作 平稳 变化 ,因此 , 若 不 受 外 界 干扰 影响 , Baxter 机 
器 人 对 应 的 角度 会 作出 相同 的 变化 。 
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本 章节 设计 实验 去 检验 基于 函数 逼近 法 的 控制 器 的 控制 效果 。 实 验 使 用 每 个 臂 7 自由 
KERDU Baxter 机 器 人 。 操 作者 采取 匀速 周期 性 运动 。 本 次 实验 中 选取 4 个 主要 的 关节 
测试 ,分 别 为 户 部 偏 移 角 、 肩 部 转动 角 、 肘 部 偏 移 角 和 肘 部 转动 角 。 因 此 选择 这 4 个 关节 数 
据 检 验 结果 。 采 用 前 面 章节 的 实验 方法 获取 人 体 关 节 数 据 ,通过 变换 矩阵 了 和 偏 移 和 矩阵 B 
映射 到 机 器 人 端 。 将 关节 角度 送 至 Python 代码 后 ,使 用 转 矩 模式 控制 Baxter 机 器 人 。 将 
人 体 关 节 看 作 参 考 信号 ,Baxter 机 器 人 角度 则 看 作 实际 的 反馈 信和 号。 

基于 FAT 的 控制 器 实验 结果 如 图 7-44 一 图 7-49 所 示 。 图 中 的 g 表示 实际 的 关节 角度 
和 qd 参考 的 关节 角度 ,分 别 代 表 肩 部 偏 移 角 、 肩 部 转动 角 、 肝 部 偏 移 角 和 肘 部 转动 角 。 图 7-48 
展示 了 FAT 中 4 个 关节 权 和 矩阵 G 的 收敛 性 。 图 7-49 展示 了 由 FAT 获得 的 补偿 转 矩 8 一 
G--Mé 十 C,。 可 知 ,补偿 转 和 矩 不 为 0。 由 图 7-48 和 图 7-49 可 知 ,关节 角度 误差 逐渐 变 小 ， 
并 且 该 神经 网 络 权 值 收敛 ,这 可 以 说 明 提出 算法 的 有 效 性 。 
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Fd 7-44 FAT 控制 器 下 的 关节 角度 qdl 和 实际 角度 ql 
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图 7-45 FAT 控 制 器 下 的 关节 角度 qd2 和 实际 角度 q2 
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图 7-46 FAT 控制 器 下 的 关节 角度 qd3 和 实际 角度 q3 
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图 7-48 FAT 中 四 个 关节 的 G 权重 矩阵 的 趋势 
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本 章 小 结 


本 章 介绍 了 机 器 人 操作 系统 在 Baxter 机 器 人 上 的 应 用 实例 ,包括 了 如 何 使 用 ROS 对 
机 器 人 进行 远程 控制 ,如 何 借助 ROS 在 机 器 人 上 实现 控制 算法 等 课题 。 
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